Testsoftware
(EN google-translate)
(PL google-translate)
Die Testsoftware erledigt zwei Aufgaben:
Wird der Einachser hingelegt, geht er in einen Standby-Modus. Wird er aufgerichtet, beginnt der Regelalgorithmus zu arbeiten:
Zyklisch werden Sensorsignale über die I2C-Schnittstelle vom Sensor MPU6050 geholt und daraus das Stellsignal für die Servos zum Verhindern eines Verkippens gebildet.
Realisiert wurde hier ein PI-Regler, wobei als Besonderheit hier der P-Anteil quadriert wird, um dem Umstand Rechnung zu tragen, dass das Gefährt bei großen Abweichungen überproportional reagieren sollte, aber nahe an der Senkrechten sehr viel vorsichtiger.
Achtung: Es werden Offsets von +/-20 auf das PWM-Signal gegeben, um eine Ungenauigkeit beim Fixieren der Servo-Mittelstellung auszugleichen. Diese Werte müßten von Serco zu Servo angepaßt werden.
Es werden im aktuellen Programm lediglich die Beschleunigungsdaten als Rohdaten verwendet, aber nicht die Gyro-Daten.
einachser025.zip - Testsoftware
//Mittelwert omega=0.0348
//0.0485
//-0.0063
//LEDs:
//rot A0+ A1-
//gelb A2+ A3-
//grün A4+ A5-
//A0 = D18
//A1 = D19
//A2 = D20
//A3 = D21
//A4 = D22
//A5 = D23
#include<math.h>
#include<Wire.h>
#define GYROSCALE 131.0
#define GYROOFFSET 2.71 //korrigierter Gyrooffset
//4.294.967.295 unsigned long
// 1.000.000 == 1s
//4.000.000.000 == 4000s
//1s == 1000ms == 1.000.000us
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double ax,ay,az;
double arx,ary,arz,arx_alt;
double wx,wy,wz; //Winkelgeschwindigkeiten
double integ_arx=0.0;
double stellgroesse;
boolean SERIELL = false;
double m=0.0;
#include "Antrieb.h"
Antrieb antrieb;
void rot()
{
digitalWrite(18,1);
digitalWrite(20,0);
digitalWrite(22,0);
}
void gelb()
{
digitalWrite(18,0);
digitalWrite(20,1);
digitalWrite(22,0);
}
void gruen()
{
digitalWrite(18,0);
digitalWrite(20,0);
digitalWrite(22,1);
}
void aus()
{
digitalWrite(18,0);
digitalWrite(20,0);
digitalWrite(22,0);
}
void rotgruengelb()
{
digitalWrite(18,1);
digitalWrite(20,1);
digitalWrite(22,1);
}
void setup()
{
pinMode(0,OUTPUT);
digitalWrite(0,0); //AD0 auf Masse
pinMode(1,INPUT);
pinMode(4,INPUT); //GND für MPU6050
pinMode(5,INPUT);
pinMode(6,INPUT);
pinMode(7,INPUT);
pinMode(8,INPUT);
pinMode(9,INPUT);
pinMode(10,INPUT);
pinMode(11,INPUT);
pinMode(12,INPUT);
pinMode(13,INPUT);
// put your setup code here, to run once:
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(9600);
pinMode(18,OUTPUT);
pinMode(19,OUTPUT);
pinMode(20,OUTPUT);
pinMode(21,OUTPUT);
pinMode(22,OUTPUT);
pinMode(23,OUTPUT);
digitalWrite(18,0);
digitalWrite(19,0);
digitalWrite(20,0);
digitalWrite(21,0);
digitalWrite(22,0);
digitalWrite(23,0);
antrieb.start();
arx_alt=0.0;
}
void loop()
{
// put your main code here, to run repeatedly:
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
ax = (double)AcX;
ay = (double)AcY;
az = (double)AcZ;
arx = ax / sqrt(ay*ay + az*az);
if(arx>arx_alt+0.03 || arx<arx_alt-0.03)
arx = 0.82*arx_alt + 0.18*arx;
stellgroesse = 18000.0*arx*arx;
if(arx<0.0)
stellgroesse = -stellgroesse;
stellgroesse += integ_arx*10.0;
arx_alt = arx;
integ_arx+=arx;
if(stellgroesse>480.0)
stellgroesse=480.0;
if(stellgroesse<-480.0)
stellgroesse=-480.0;
if(SERIELL)
{
Serial.println(" Berechnete Daten ohne atan: ");
Serial.print(" arx = "); Serial.println(arx,DEC);
Serial.print(" wz = "); Serial.println(wz,DEC);
Serial.print(" stellgroesse = "); Serial.println(stellgroesse,DEC);
delay(333);
}
if(arx<-0.7 || arx>0.7)
{
rot();
antrieb.links(-20);
antrieb.rechts(20);
integ_arx=0.0;
}
else
{
gruen();
antrieb.links((int)stellgroesse-20);
antrieb.rechts((int)stellgroesse+20);
}
//Auswertung:
//es wird nur wz und arx benötigt
//Drehung in Fahrtrichtung: wz<0 (Mathematisch positiver Sinn)
//Verkippung in Fahrtrichtung: arx<0 (Mathematisch positiver Sinn)
//OFFSET wz: ungefähr -2.6
}
Code 0-1: einachser025.ino - Hauptprogramm
#include "Arduino.h"
#define ANTRIEB_WMIN 1000
#define ANTRIEB_WMITTE 1500
#define ANTRIEB_WMAX 2000
#define ANTRIEB_SCHRITTE 1000
#define ANTRIEB_BEREICH 500
//Mode 8
//Phasen- und Frequenz-korrekt
//WGM1 3 2 1 0
// 1 0 0 0
//ICR1=..... TOP
//fpwm = fclk/(2*N*TOP)
//Vorteilung
//N=8
//80Hz = 16000000/(2*8*TOP)
//TOP = 16000000/(2*8*80Hz)=12500
//dt==1000ms*(1/80Hz)/12500 == 0,001ms (1 Schritt == 0,001ms)
//=>
//1ms == 1000 Schritte
//1,5ms == 1500 Schritte
//2ms == 2000 Schritte
class Antrieb
{
public:
Antrieb()
{
}
void start()
{
TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (0<<COM1C1) | (0<<COM1C0) | (0<<WGM11) | (0<<WGM10);
TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (0<<WGM12) | (0<<CS12) | (1<<CS11) | (0<<CS10); //Vort. 256, s.S. 125
ICR1=12500;
DDRB |= (1<<PB5); //OCR1A
DDRB |= (1<<PB6); //OCR1B
OCR1A = ANTRIEB_WMITTE; //PWM-Breite auf Mitte setzen.
OCR1B = ANTRIEB_WMITTE; //PWM-Breite auf Mitte setzen.
}
//x == -500...+500
void links(int x)
{
if(x<-ANTRIEB_BEREICH)
x=-ANTRIEB_BEREICH;
if(x>ANTRIEB_BEREICH)
x=ANTRIEB_BEREICH;
OCR1A = ANTRIEB_WMITTE+x;
}
void rechts(int x)
{
if(x<-ANTRIEB_BEREICH)
x=-ANTRIEB_BEREICH;
if(x>ANTRIEB_BEREICH)
x=ANTRIEB_BEREICH;
OCR1B = ANTRIEB_WMITTE-x;
}
};
Code 0-2: Antrieb.h - Klasse zur Ansteuerung der Antriebsservos