Hello everyone!
I have been trying to achieve balance in my self balancing robot; however, the robot does not want to balance... I've tried everything... It would be nice to read about your ideas.
My hardware:
Pololu 30:1 37D motors with encoders
MPU 9250 9DOF Sensor
Arduino Uno
VNH5019 dual motor drivers from Pololu
120 mm all terrain wheels
3S 11.1V Lipo, 350grams
Picture of the robot attached.
Also I've seen some videos explaining the PID tuning process on these kinds of robots, and they did not help a whole lot. I can get the robot to shake a lot or oscilate for like 2 seconds and then fall. Also I've implemented a Kalman filter, made the sampling time 5ms, and no luck...
Thanks for your time in advance, this is driving me crazy. I need some advice.
This is my code:
#include <Kalman.h>
#include "MPU6050.h"
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
//Ports and pins used to decide the direction of the motors
#define DIR_PORT PORTD
#define M1_ENA PD4
#define M1_ENB PD5
#define M2_ENA PD6
#define M2_ENB PD7
//PID variables
double angle_wanted, angle_present, iTerm, dTerm , error, last_error = 0 ;
double pwm_out;
bool DIRECTION;
double Min = -511, Max = 511; //Min and Max that the PWM can take
double Kp = 43 , Ki = 0, Kd = 10; //PID PARAMETERS
int sample_time_ms = 5;
double sample_time_s;
unsigned long now, then, time_wait;//Fixed loop vars
int16_t ax, ay, az, gx, gy, gz;
double time01Step, time01, time01Prev;
double rx;
double gyroScale = 131;
Kalman kalman;
MPU6050 accelgyro;
void setup() {
pinMode ( 9, OUTPUT); // PWM Motor 1
pinMode ( 10, OUTPUT); // PWM Motor 2
pinMode ( 4, OUTPUT); // Enable CW Motor 1
pinMode ( 5, OUTPUT); // Enable CCW Motor 1
pinMode ( 6, OUTPUT); // Enable CW Motor 2
pinMode ( 7, OUTPUT); // Enable CCW Motor 2
pinMode ( 13, OUTPUT); // Loop time indicator
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM12) | _BV(WGM11);
TCCR1B = _BV(CS10);
// ( Kp , Ki, Kd )
// pidTune( 31 , 0 , 0 );
Wire.begin(); // Begin the IIC protocol to get info from IMU
Wire.setClock(400000UL);
time01 = millis();
angle_wanted = 178.53;
Serial.begin(9600);
}
void loop() {
now = millis();
angle_present = angleRead(); //Read the angle
//Serial.println(angle_present);
pwm_out = pidCalc( angle_present );//Input the read angle and then calculate output
/*if ( pwm_out >= 0 ){
pwm_out = scale( pwm_out, 0 , 511, 230 , 739);
}
else{
pwm_out = ( 0 - pwm_out );
pwm_out = scale( pwm_out, 0 , 511, 230 , 739);
pwm_out = ( 0 - pwm_out );
}
*/
/*
if ( (angle_wanted <= angle_wanted + 1) && (angle_wanted >= angle_wanted - 1 )){
pwm_out = 0;
}*/
//Serial.println( pwm_out );
//Serial.println( pwm_out );
// Serial.println(error);
if ( (error < 45)&&(error > -45)){ // Stop the motors when the angle is over 45
motorSpeed ( 9 , pwm_out );
motorSpeed ( 10 , pwm_out );
// Serial.println("HAHAHAHA");
}
else{
//Serial.println("NAAAAH");
motorSpeed ( 9 , 0 );
motorSpeed ( 10 , 0 );
}
then = millis();// Fixed loop time
time_wait = sample_time_ms - (then - now);
if (time_wait > 0 ){
PORTB = B100000;
delay(time_wait);//Indicate in LED 13 that the loop has time to run
}
else{
PORTB = B000000;
}
}
//Scale formula from TKJ Electronics
double scale(double input, double inputMin, double inputMax, double outputMin, double outputMax) { // Like map() just returns a double
double output;
if(inputMin < inputMax)
output = (input-inputMin)/((inputMax-inputMin)/(outputMax-outputMin));
else
output = (inputMin-input)/((inputMin-inputMax)/(outputMax-outputMin));
if(output > outputMax)
output = outputMax;
else if(output < outputMin)
output = outputMin;
return output;
}
//Configure PWM
void setPWM(uint8_t pin, int dutyCycle) {
if ( pin == 9 ){
OCR1A = dutyCycle;
}
else if ( pin == 10){
OCR1B = dutyCycle;
}
}
//Drive the motors, it can take from -511 to 511 and then set the motors accordingly
void motorSpeed(uint8_t pin_motor, int Speed){
if ( Speed >= 0 ){
DIRECTION = true;
}else{
DIRECTION = false;
Speed = ( 0 - Speed );
}
if ( pin_motor == 9 ){
setPWM( 9 , Speed );
if ( DIRECTION == true ) { //
sbi( DIR_PORT , M1_ENA );
cbi( DIR_PORT , M1_ENB );
}else{ //
cbi( DIR_PORT , M1_ENA );
sbi( DIR_PORT , M1_ENB );
}
}else if ( pin_motor == 10 ){
setPWM( 10, Speed );
if ( DIRECTION == true ) { //
sbi( DIR_PORT , M2_ENA );
cbi( DIR_PORT , M2_ENB );
}else{ //
cbi( DIR_PORT , M2_ENA );
sbi( DIR_PORT , M2_ENB );
}
}
}
/*
float calibrateSensors(){
double zeroval;
for (int j=0; j<100 ; j++){
zeroval+= angleRead();
}
zeroval/=100;
return zeroval;
}
*/
double pidCalc( double angle_read ){//Calculation of the PID values
error = angle_wanted - angle_read;
//Serial.println( error );
iTerm += ( Ki * error );
if ( iTerm > Max) iTerm = Max;
else if ( iTerm < Min ) iTerm = Min;
dTerm = Kd * ( error - last_error );
double out = Kp * error + iTerm + dTerm ;//PID complete
//Serial.println( out );
if ( out > Max ) out = Max;//SAturate the output of the controller
else if ( out < Min ) out = Min;
last_error = error;
//Serial.println(out);
return out;
}
float angleRead(){
time01Prev = time01;
time01 = millis();
time01Step = (time01 - time01Prev) / 1000; //Calculate timestep
//Get the IMU data from the MPU 9250 same as MPU6050
ax = accelgyro.getAccelerationX();
az = accelgyro.getAccelerationZ();
gx = accelgyro.getRotationX();
float gyroRate = -((double)((double)gx) / 1.0323);
// apply gyro scale from datasheet
gyroRate = gyroRate/gyroScale;
//gyroAngle += gyroRate * ((double)(time01Step));
float newangle = (atan2(-ax,-az)+PI)*RAD_TO_DEG;
rx = kalman.getAngle( newangle , gyroRate , time01Step ); //Kalman Filter by TKJ Electronics
// print result
// Serial.println(rx);
return rx;
