Hello all,
I too am building a self balancing robot like a lot of you members.
The hardware I'm running is:
Adafruit 10-DOF IMU (Adafruit 10-DOF IMU Breakout - L3GD20H + LSM303 + BMP180 : ID 1604 : $29.95 : Adafruit Industries, Unique & fun DIY electronics and kits)
Arduino Uno R3
Standard hobby geared motors
Adafruit motor driver shield v2.3 (https://www.adafruit.com/products/1438)
I'm reading the '.roll' field from the gyro and using that as an input for the bot.
Yes, I'm using only the gyro values for balancing (tried using the accelerometer, but the bot used to go haywire, even with the complimentary filter)
A little bit of help in this would also be great.
I'm using Brett Beauregard's PID library for the PID control.
My PID constants are:
Ki = 6.9
Kp = 4.3
Kd = 0.35
The bot balances well for small errors (<= 5deg), but after that it goes crazy, with sudden oscillations and jerks around.
One thing that I've noticed (in the serial monitor) is that when the motor start to correct the lean/tilt/error, after one or two cycles the motor gets a sudden large input (255 for the PWM pulse). I can't seem to get rid if that 'noise' in the output signal no matter what I do.
Right now I'm running the code at 100ms delay, as any other delay causes erratic behavior of the bot.
I'm attaching my code along with this post, and soon will upload a video showing the robot's behavior too.
Edit: The video is up on youtube: Balancing robot issues. - YouTube
Edit2, the code. Decided to post the code onto the post itself:
#include <Wire.h>
#include <PID_v1.h>
#include <Adafruit_10DOF.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_Simple_AHRS.h>
#include <Adafruit_MotorShield.h>
//Create sensor instances.
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
//Create simple AHRS algorithm using the above sensors.
Adafruit_Simple_AHRS ahrs(&accel, &mag);
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myLeftMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myRightMotor = AFMS.getMotor(2);
double Ki; //take analog input for manual setting of ki,kp,kd
double Kp;
double Kd;
//Define Variables we'll be connecting to
double Setpoint, Input, Output, Input1;
//Specify the links and initial tuning parameters
PID PID_A(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE);
int a = 100; //Set delay period in ms
void setup()
{
Serial.begin(115200);
//Initialize the sensors
accel.begin();
mag.begin();
AFMS.begin();//create with the default frequency 1.6KHz
//Set the speed to start, from 0 (off) to 255 (max speed)
myLeftMotor->setSpeed(0); myRightMotor->setSpeed(0);
myLeftMotor->run(FORWARD); myRightMotor->run(FORWARD);
//turn on motor
myLeftMotor->run(RELEASE); myRightMotor->run(RELEASE);
//initialize the variables we're linked to
Setpoint = -1.0;
PID_A.SetSampleTime(a);
PID_A.SetMode(AUTOMATIC);//turn the PID on
PID_A.SetOutputLimits(-255,255);
}
void loop(void)
{
double K1 = 0.0;
double K2 = 0.0;
double K3 = 0.0;
double Pot1 = A0;
double Pot2 = A1;
double Pot3 = A2;
double Roll;
double MotorSpeed;
sensors_vec_t orientation;
boolean Res = (ahrs.getOrientation(&orientation));
Roll = orientation.roll;
/*sensors_event_t event;
accel.getEvent(&event);
double Ay = event.acceleration.y;
double Angle = 0.98*(Roll + Ay*0.01) + 0.02*Ay;*/
Input = Roll;
K1 = analogRead(Pot1);
K2 = analogRead(Pot2);
K3 = analogRead(Pot3);
K1 = map(K1, 0, 1023, 0, 100);
K2 = map(K2, 0, 1023, 0, 100);
K3 = ((0.9*K3)/1023) + 0.09;
K1 = K1/10;
K2 = K2/10;
//K3 = K3/10;
Serial.print("Input: ");
Serial.print(Input);
Ki = K1;
Kp = K2;
Kd = K3;
PID_A.SetTunings(Ki, Kp, Kd);
PID_A.Compute();
Serial.print(" MotorSpeed: ");
Serial.println(Output);
Serial.print(" Ki ");
Serial.print(Ki);
Serial.print(" Kp ");
Serial.print(Kp);
Serial.print(" Kd ");
Serial.println(Kd);
if(Output < 0)
{ MotorSpeed = -Output;
myLeftMotor->setSpeed(MotorSpeed);
myLeftMotor->run(BACKWARD);
myRightMotor->setSpeed(MotorSpeed);
myRightMotor->run(BACKWARD);
/*Serial.print("MotorSpeed: ");
Serial.print(MotorSpeed);*/
//delay(a);
}
else
{ MotorSpeed = Output;
myLeftMotor->setSpeed(MotorSpeed);
myLeftMotor->run(FORWARD);
myRightMotor->setSpeed(MotorSpeed);
myRightMotor->run(FORWARD);
// delay(a);
}
delay(a);
}
Balancing_Robot3.ino (2.75 KB)
