Projekt: Raspi motor Controller mit Rotationsencodern, PID control und mehr
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, ¶m);
pthread_create(&thread1, NULL, thread1Go, NULL);
param.sched_priority = 25;
pthread_setschedparam(thread1, SCHED_RR, ¶m);
pthread_create(&thread2, NULL, thread2Go, NULL);
param.sched_priority = 50;
pthread_setschedparam(thread2, SCHED_RR, ¶m);
pthread_create(&thread3, NULL, thread3Go, NULL);
param.sched_priority = 90;
pthread_setschedparam(thread3, SCHED_RR, ¶m);
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!
