-         

Ergebnis 1 bis 2 von 2

Thema: Projekt: Raspi motor Controller mit Rotationsencodern, PID control und mehr

  1. #1
    Erfahrener Benutzer Robotik Einstein
    Registriert seit
    09.10.2014
    Beiträge
    2.452

    Projekt: Raspi motor Controller mit Rotationsencodern, PID control und mehr

    Anzeige

    SMARTPHONES & TABLETS-bis zu 77% RABATT-Kostenlose Lieferung-Aktuell | Cool | Unentbehrlich
    hallo,
    ich habe vor, auf der Basis dieses Motor-Encoder-Codes einen PID controller zu schreiben. Da hier arrays of struct verwendet werden, ist der Code skalierbar; da nun aber der Raspi recht wenige Pins hat und 1 Motor bereits 5 pins benötigt, sollen auch Huckepack-Arduinos mit einbezogen werden können als Encodermotor-Multiplexer.


    Code:
    // encoder test
    // wiringPi,   
    // Encoder Timer High Priority Thread
    // ver 0007
    
    // protected under the Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
    // http://creativecommons.org/licenses/by-nc-sa/3.0/
    
    #include <stdio.h>
    #include <stdlib.h>
    #include <unistd.h>
    #include <math.h>
    #include <fcntl.h>
    #include <string.h>
    #include <sys/ioctl.h>
    #include <stdint.h>
    #include <errno.h>
    #include <pthread.h>
    #include <termios.h>
    
    #include "VG/openvg.h"
    #include "VG/vgu.h"
    #include "fontinfo.h"
    #include "shapes.h"
    
    #include <wiringPi.h>
    #include <wiringSerial.h>
    
    
    #define  byte  uint8_t;
    
    #define  MAXMOTORS    2   // max number of encoder motors
    
    typedef struct {
          uint8_t    pd1, pd2, pwm;
          uint8_t    pqa, pqb;
          int32_t    motenc, oldenc;
    
    }  tpimotor ;
    
    tpimotor  motor[MAXMOTORS];
    
                                   
    /*************************************************************
    * Encoder Handler Routine
    *************************************************************/
    
    volatile int8_t ISRab[MAXMOTORS];
    
    // 1/2 resolution
    int8_t enctab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};
    
    void updateEncoders() {   
      int i;    
      for( i=0; i<MAXMOTORS; ++i ) {   
        ISRab [ i] <<= 2;
        ISRab [ i] &=  0b00001100;
        ISRab [ i] |= (digitalRead( motor[ i].pqa ) << 1) | digitalRead(  motor[ i].pqb );
        motor[ i].motenc += enctab[ ISRab[ i] ];   
      }     
    
    }
    
    
                                 
    
    void* thread3Go(void *)   // encoder high priority thread
    {
       while(1) {
         updateEncoders();
         usleep(10);
       }         
       return NULL;
    }
    
    void* thread2Go(void *)
    {
       while(1) {
          delay(10);
       }         
       return NULL;
    }
    
    void* thread1Go(void *)   // low priority display thread
    {
       char  sbuf[128];
       while(1) {   
         delay(100);
       }         
       return NULL;
    }
    
    void* thread0Go(void *)
    {
       char  sbuf[128];
       while(1) {
        sprintf(sbuf, " 0=%6ld    1=%6ld \n ",  motor[0].motenc, motor[1].motenc );
         printf(sbuf);
         delay(100);
       }         
       return NULL;
    }
    
    
    
    
    void setup() {
      int i;   
       
      // motor pin settings
      // encoder pin settings
      // setup for L293D motor driver
    
      // motor 0
     
         motor[0].pqa = 4;
         motor[0].pqb = 5;
         motor[0].pd1 =21;
         motor[0].pd2 =22;
         motor[0].pwm =23;
         
         motor[1].pqa = 0;
         motor[1].pqb = 2;
         motor[1].pd1 =24;
         motor[1].pd2 =25;
         motor[1].pwm =26;   
         
         for( i=0; i< MAXMOTORS; ++i)  {       
            pinMode(motor[i].pqa, INPUT);        // encA   
            pinMode(motor[i].pqb, INPUT);        // encB   
            pinMode(motor[i].pd1, OUTPUT);       // dir-1   
            pinMode(motor[i].pd2, OUTPUT);       // dir-2   
            pinMode(motor[i].pwm ,OUTPUT);       // pwm
           
            motor[i].motenc = 0;
            motor[i].oldenc = 0;
            ISRab[i] = 0;
       }
    }
    
    
    
    
    
    int main() {
        char  sbuf[128];
        pthread_t thread0, thread1, thread2, thread3;
       
        wiringPiSetup();   
        if(wiringPiSetup() == -1) return 1;   
       
        setup();
       
        struct sched_param param;
       
       
        pthread_create(&thread0, NULL, thread0Go, NULL);
        param.sched_priority = 10;
        pthread_setschedparam(thread0, SCHED_RR, &param);
       
        pthread_create(&thread1, NULL, thread1Go, NULL);
        param.sched_priority = 25;
        pthread_setschedparam(thread1, SCHED_RR, &param);
       
        pthread_create(&thread2, NULL, thread2Go, NULL);
        param.sched_priority = 50;
        pthread_setschedparam(thread2, SCHED_RR, &param);
       
        pthread_create(&thread3, NULL, thread3Go, NULL);
        param.sched_priority = 90;
        pthread_setschedparam(thread3, SCHED_RR, &param);
       
       
        pthread_join(thread0, NULL);
        pthread_join(thread1, NULL);
        pthread_join(thread2, NULL);
        pthread_join(thread3, NULL);
       
        exit(0);
       
    }
    Die sehr minimalistische Struktur sieht hier jetzt noch so aus:
    Code:
    #define  MAXMOTORS    2   // max number of encoder motors
    
    typedef struct {
          uint8_t    pd1, pd2, pwm;   // H-Bridge pins
          uint8_t    pqa, pqb;        // encoder pins
          int32_t    motenc, oldenc;  // encoder values
    
    }  tpimotor ;
    
    tpimotor  motor[MAXMOTORS];
    Etwas ähnliches habe ich bereits für den Lego NXT programmiert (NXC), aber damals hatte ich nicht alle meine heutigen Möglichkeiten...
    alle PID Funktionen sollen aber jetzt ebenfalls über eine skalierbare motor struct gesteuert werden und in eigenständigen Tasks ablaufen:

    http://www.mindstormsforum.de/viewtopic.php?f=25&t=7521&p=62136#p61930


    hier ist die motor (PID) Struktur schon etwas weiter "aufgebohrt" (ohne Pin-Definitionen) und soll jetzt für den Raspi adapiert werden:

    Code:
    struct PIDstruct {
                     // custom target values
      long  target;                  // set target
      int   tarpwm;                  // motor target speed
                     // custom regulation parameters
      float P;                       // P: basic propotional to error
      float I;                       // I: integral: avoid perish
      float D;                       // D: derivative: avoid oscillating
      float precis;                  // error precision to target
      int   regtime;                 // PID loop time
      float damp;                    // damp the integral memory
      char  cont;                    // target: continue or hit once
                     // internal control variables
      char  runstate;                // monitors runstate
      int   outp;                    // PID control output value
      int   maxout;                  // max output (max motor pwr)
      long  read;                    // current sensor reading
      float err;                     // current error
      float integr;                  // integral of errors
      float speed;                   // current speed
    
    
    } ;
    Die obigen Pin-Definitionen sind jetzt hier hinein zu integrieren.

    wer möchte gerne mitmachen?
    könnte sicher ein interessantes Projekt für mobile Roboter und auch Roboterarme etc. werden!
    Geändert von HaWe (19.02.2016 um 22:49 Uhr)
    ·±≠≡≈³αγελΔΣΩ∞ Schachroboter:www.youtube.com/watch?v=Cv-yzuebC7E Rasenmäher-Robot:www.youtube.com/watch?v=z7mqnaU_9A8

  2. #2
    Erfahrener Benutzer Robotik Einstein
    Registriert seit
    09.10.2014
    Beiträge
    2.452
    update:

    die einfachen Motor-Kommandos
    motorOn (nr, dirpwm) // dirpwm: => pwm mit Vorzeichen für die Richtung
    motorCoast (nr)
    motorBrake (nr)

    funktionieren bereits.

    http://www.mindstormsforum.de/viewto...p=67815#p67780

    edit: und jetzt auch hier:
    http://www.mindstormsforum.de/viewto...p=69020#p68940
    Geändert von HaWe (28.02.2016 um 10:51 Uhr)
    ·±≠≡≈³αγελΔΣΩ∞ Schachroboter:www.youtube.com/watch?v=Cv-yzuebC7E Rasenmäher-Robot:www.youtube.com/watch?v=z7mqnaU_9A8

Ähnliche Themen

  1. Antworten: 63
    Letzter Beitrag: 18.02.2016, 08:43
  2. Tausche RN-Control 1.4 + R232 Anschlusskabel gegen ARM ( Cubie / Odroid / Raspi B+)
    Von piggituX im Forum Kaufen, Verkaufen, Tauschen, Suchen
    Antworten: 0
    Letzter Beitrag: 11.12.2014, 19:27
  3. (Unter-)Projekt Schrittmotor-Controller
    Von MeckPommER im Forum Vorstellungen+Bilder von fertigen Projekten/Bots
    Antworten: 18
    Letzter Beitrag: 01.06.2009, 14:43
  4. Welchen Controller für mein Projekt?
    Von PanicGamer im Forum ARM - 32-bit-Mikrocontroller-Architektur
    Antworten: 1
    Letzter Beitrag: 17.11.2008, 19:24
  5. Uni-Projekt: 15 Servos, LCD-Display, Schalter und Controller
    Von iNGY! im Forum Vorstellung+Bilder+Ideen zu geplanten eigenen Projekten/Bots
    Antworten: 18
    Letzter Beitrag: 24.10.2007, 01:04

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •