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

[HaWe]

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.

// 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:

#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:

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!

[HaWe]

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