Hallo,

hab jetzt auch mal kurz an einem balancierenden Robot gaearbeitet,
aber nich wie waste mit optischen Sensoren, sondern mit einem 6DOF: MPU6050 (3xacc + 3x gyro).

Wollte aber mal was anderes machen:

- nur einen Motor, eine H-Brücke verwenden,
- ohne Getriebe auskommen.

Letzteres war das Problem, da bei Drehzahl kurz über 0 auch kaum Drehmoment da ist, bei höherer Drehzahl das Fahrzeug aber Geschwindigkeit aufnimmt ...

Die Regelung musste daher recht feinfühlig sein. Den Sensor hab ich ca 2,5 cm oberhalb der Motorachse angebracht.
Der Motor ist eine Billiigmodell, an dem die Achsen an beiden Seiten herausgeführt sind, die Räder sitzen direkt auf der Motorachse.


Hab das dann - für kleine Kippwinkel - auch hinbekommen, musste aber dazu die Motorspannung auf ca 11 V (3 Lion-Akkus) erhöhen
Den Code hab ich zum grössten Teil aus diversen Foren (Kalman-Filter, PD-Regler), letzteren hab ich aber etwas angepasst, da die Kippmoment / Winkelkurve recht nichtlinera ist .

Das Video ist auf Youtube:

http://www.youtube.com/watch?v=tKy0-RIf0yQ&google_comment_id=z13gctlzyvqhgzcdo22ov1di3 ny4upq1l&google_view_type#gpluscomments


/ Balance Robot mit 6DOF MPU6050 und FastPWM_Timer2 (Pin D3 (OC2B/PD3)
// 20.12.2013 angepasst an einen Motor mit Anfahr-PWM 40
// 28.12.2013 aus Balance_22 mit GLCD-ST7735 Ausgabe zum Test
// 29.12.2013 Ausgabe auf GLCD ST7735 deaktiviert
// 29.12.2013 stabilste Version mit einem Motor an 3 Lipos
//
// PID nit komst.-Wert bei 0°
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
/*
//################################################## ##############
#define cs 10
#define dc 9
#define rst 8 // you can also connect this to the Arduino reset

#include <Adafruit_GFX.h> // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library
#include <SPI.h>

Adafruit_ST7735 tft = Adafruit_ST7735(cs, dc, rst);

//################################################## ##############
*/

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#ifndef PI

# define PI 3.141592653589793f

#endif


#ifndef M_PI_2

# define M_PI_2 1.570796326794897f

#endif


#ifndef atanf

# define atanf atan

#endif


#ifndef atan2f

# define atan2f atan2

#endif


Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
Kalman kalmanZ;
char letter;
char buf[5];
int dirPin = 4; //
int pwmPin = 3; // PWM-Pins: 3(OC2B) (servo),11(OC2A,MOSI); 5(OC0B),6 (OC0A)(delay); 9(OC1A),10 (SS)
//int val = 0; //

int ir,il,ipwm;
int pwma= 0,pwmn=0;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
//int16_t ikal=0;
float /*double*/ accXangle, accYangle,accZangle; // Angle calculate using the accelerometer
float /*double*/ temp; // Temperature
float /*double*/ gyroXangle, gyroYangle,gyroZangle; // Angle calculate using the gyro
float /*double*/ compAngleX, compAngleY,compAngleZ; // Calculate the angle using a complementary filter
float /*double*/ kalAngleX, kalAngleY,kalAngleZ; // Calculate the angle using a Kalman filter
int ita,ikal,ikam,ikala=0,ikaln,iacc;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
uint8_t itap;
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

// PID
const float Kc = 0; //8,10,20,40,60,0
const float Kp = 5; //10,8,19;16,14,12,4,5,7,5,6,8,20,30,50,30,54,60,50, 51,48,53,50
const float Ki = 4; //8,5,14,17,8,15,12,10
const float Kd = 2;//5,3,4,10,6,3,,8,11,4,9,7,5
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0

void setup()
{
/*
//############################
tft.initR(INITR_BLACKTAB);
tft.fillScreen(VGA_SILVER);
//############################
*/
pinMode(dirPin, OUTPUT); // setzt den Pin als Output
pinMode(pwmPin, OUTPUT); // setzt den Pin als Output
//================================================== ===========
//==================== Fast-PWM Timer2 ========================
//================================================== ===========
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20); // A frequency: 16/64/256 = 976Hz
// TCCR2B = _BV(CS22); // A frequency: 16MHz/64/256 = 976Hz
TCCR2B &= ~(1<<CS01); // Invert that to "11111101" // 4kHz
// TCCR2B &= ~(1<<CS00); // Invert that to "11111110" // 32kHz
// OCR2A = 180; // Output A/D11 Duty Cycle: (180+1) / 256 = 70.7%
OCR2B = 40; // Anfahr-PWM
// Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode

while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x6 { // Read "WHO_AM_I" register
// Serial.print(F("Error reading sensor"));
while(1);
}

delay(100); // Wait for sensor to stabilize

/* Set kalman and gyro starting angle */
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << | i2cData[1]);
accY = ((i2cData[2] << | i2cData[3]);
accZ = ((i2cData[4] << | i2cData[5]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accZangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
kalmanZ.setAngle(accZangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
gyroZangle = accZangle;
compAngleX = accXangle;
compAngleY = accYangle;
compAngleZ = accZangle;
timer = micros();


}

int Pid(){
int ispeed,it;
// error = error - 180; // 180 = level??
it = (int) error;
if (it < 0) it = -it;
// weil das Kippmoment sehr nichtlinear ist (Sinuskurve), hab ich de bei grössern Winkeln
// den Fehler vergröösert:
if (it <= 1) ;
else if (it <= 2) error = error * 1.1; // 1.1;// 1.1;
else if (it == 3) error = error * 1.2; //1.3; //1,2;
else if (it == 4) error = error * 1.4; //1.6; //1.4;
else if (it == 5) error = error * 1.7; //2.0; //1.7;
else if (it == 6) error = error * 2.0; //2.5; //2.0;
else if (it == 7) error = error * 2.4; //3.1; //2.4;
else if (it <= 10) error = error * 4.0; //5.5; //4.0
else if (it <= 20) error = error * 10.0;
// else error = error * 40.0;
pTerm = Kp * error; // + Kc;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
ispeed = constrain(K*(pTerm + iTerm + dTerm), -215, 215);
return(ispeed);
}


uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop)
{
return i2cWrite(registerAddress,&data,1,sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop)
{
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
// Serial.print(F("i2cWrite failed: "));
// Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}



uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
// Serial.print(F("i2cRead failed: "));
// Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes,(uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
// Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}





void loop()
{


/* Update all the values */

while(i2cRead(0x3B,i2cData,14));


accX = ((i2cData[0] << | i2cData[1]);
accY = ((i2cData[2] << | i2cData[3]);
accZ = ((i2cData[4] << | i2cData[5]);
// tempRaw = ((i2cData[6] << | i2cData[7]);
gyroX = ((i2cData[8] << | i2cData[9]);
gyroY = ((i2cData[10] << | i2cData[11]);
gyroZ = ((i2cData[12] << | i2cData[13]);

// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
// accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
// accZangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;

float /*double*/ gyroXrate = (float/*double*/)gyroX/131.0;
// double gyroYrate = -((double)gyroY/131.0);
// double gyroZrate = -((double)gyroZ/131.0); // ??
gyroXangle += gyroXrate*((float/*double*/)(micros()-timer)/1000000); // Calculate gyro angle without any filter
// gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
// gyroZangle += gyroZrate*((double)(micros()-timer)/1000000); // ???
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
// compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
// compAngleZ = (0.93*(compAngleZ+(gyroZrate*(double)(micros()-timer)/1000000)))+(0.07*accZangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float/*double*/)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
// kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
// kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
timer = micros();

// temp = ((double)tempRaw + 12412.0) / 340.0;

/*
Serial.print(" xAcc: "); Serial.print(accXangle);
Serial.print(" xGyr: "); Serial.print(gyroXangle);
Serial.print(" xCom: "); Serial.print(compAngleX);
Serial.print(" xKal: "); Serial.print(kalAngleX);
// Serial.print(" yAcc: "); Serial.print(accYangle);
// Serial.print(" yGyr: "); Serial.print(gyroYangle);
// Serial.print(" yCom: "); Serial.print(compAngleY);
// Serial.print(" yKal: "); Serial.println(kalAngleY);

// Serial.print(" zAcc: "); Serial.print(accZangle);
// Serial.print(" zGyr: "); Serial.print(gyrZAngle);
// Serial.print(" zCom: "); Serial.print(compAngleZ);
// Serial.print(" zKal: "); Serial.print(kalAngleZ);

*/
error = kalAngleX;//accXangle;//kalAngleX; // laAngleX = 180° (Fahrzeug senkrecht)
error = error - 180;
ikal = (int)error; //accXangle;//kalAngleX; // kal ??
// Serial.print(" ikal: "); Serial.print(ikal);
// ikal = ikal - 180;
// Serial.print(" ikal-180: "); Serial.println(ikal);
pwma = pwmn;

if (ikal > 0)
{
ikaln = ikal;
digitalWrite(dirPin, LOW);
ipwm = Pid();//mot_pwm();
//ipwm = ipwm; // PWM: 0-255

}
else
{
if (ikal < 0)
{
// ikal = -ikal;
ikaln = ikal;
digitalWrite(dirPin, HIGH);
ipwm = Pid(); //mot_pwm();
ipwm = -ipwm; // PWM: 0-255

} // ikal < 0
else ipwm = 0;//205; //0; //0;
// if ((iacc - ikal) > 5 ) OCR2B = OCR2B + 3;

}
// if (ipwm > 215) ipwm = 215;
ipwm = ipwm + 40; // Anfahr-PWM
pwmn = ipwm;

if (ikal > 70) ipwm = 0;
if (ikal < -70) ipwm = 0;
OCR2B = (int)ipwm;

//delay(2);//1),//+ikal);//10,12,16/15/18/10/20delay(2); //10
ikala = ikaln;
//================================================== =====================
/*
Serial.print(" xAcc: "); Serial.print(accXangle);
// Serial.print(" xGyr: "); Serial.print(gyroXangle);
// Serial.print(" xCom: "); Serial.print(compAngleX);
Serial.print(" xKal: "); Serial.print(kalAngleX);
Serial.print(" error: "); Serial.print(error);
Serial.print(" iKal: "); Serial.print(ikal);
Serial.print(" OCR2B: "); Serial.println(OCR2B);
// delay(200);
*/
//
//################################################## ######################################
/*
tft.fillRect(52,5,53,114,VGA_SILVER);//GRAY);
tft.setTextColor(ST7735_BLACK,VGA_SILVER);
tft.setCursor(2,5); tft.print("x-accel:"); tft.setCursor(52, 5); tft.print(accXangle,2);
tft.setCursor(2,20); tft.print("x-gyro:"); tft.setCursor(52,20); tft.print(gyroXangle,2);
tft.setCursor(2,35); tft.print("x-comp:"); tft.setCursor(52,35); tft.print(compAngleX,2);
tft.setCursor(2,50); tft.print("x-kalm:"); tft.setCursor(52,50); tft.print(kalAngleX,2);
tft.setCursor(2,70); tft.print("error:"); tft.setCursor(52,70); tft.print(error,2);
tft.setCursor(2,90); tft.print("ikal:") ; tft.setCursor(52,90); tft.print(ikal);

tft.setCursor(2,110); tft.print("OCR2B:"); tft.setCursor(52,110);
if (OCR2B > 200) tft.setTextColor(ST7735_RED,ST7735_WHITE);
tft.print(0,10);tft.setCursor(52,110); tft.print(OCR2B,10);

// delay(1);
//################################################## ####################
*/


}