The code is almostthe same as we had for the brushless PID controller or the ping pong ball controller, but this time we use stepper motors. Top create the step pulses applied to the stepper drivers, we use a timer, TIEMR 2 in this case. By setting some registers, we make that timer to click every 20us. We read the raw data from the gyro and accelerometer. To overcome the gyro offset, at the begginning, we make around 500 reads from the IMU module and save that data as offset and subtract it later in the main loop. Then we calculate the angle each loop which is set to be each 4000us. Using the "gyro_angle" variable, we can calculate the PID value. As always, we find the PID error which is the difference between the real angle of the robot and the desired angle, which should be 0 so the robot would stay horizontal. Finally, inside the timer routine, depending on the PID output, we calculate the speed of the motors by creating more or less, faster or slower pulses applied to the stepper motors. To make the code faster, instead of digital read we use register control where this line for example: PORTD |= 0b00001000; is the same as setting the digital pin D3 to HIGH. For more about this, take a look at my Arduino register control video.
My robot is still wobbling around a little bit. That’s because we have to adjust the PID values in the code. This was the longest part of this project. I had to manually go and change the PID values till I got better results and even so, I’m not quite happy with the final results. If you start only with P values and the I and D are 0, you get some kind of oscillations where the wheels are rotating in the same direction as the tilt of the robot according to the angle. Then we add some D values and it will react to the speed of changing of the angle, and try to stop the oscillation, but that's not enough when the error is very small. Finally, to fine adjust the position, we add the I that will increase or decrease slowly when the error is small.Aftter more than 30 tries, I've got more or less good results and the robot is quite stable but I'm not entirely happy yet.
/* This code is for the balancing robot project. You can find more about this on https://www.electronoobs.com.
* The code will read data from an IMU module, calculate the PID value in order to balance the robot at 0º and
* then it will create pules using a timer and apply those to some stepper motors and move the robot.
* Kind thanks to Joop Brokking for the help: https://www.youtube.com/user/MacPuffdog
*
* Tutorial: https://electronoobs.com/eng_arduino_tut159.php
* Schematic: https://electronoobs.com/eng_arduino_tut150_sch1.php */
//Include Libraries
#include <Wire.h> //Wire.h is used to get i2c data from the MPU6050
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID VALUES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
float Kp = 30; //P Gain; Mine was 30
float Ki = 0.61; //I Gain; Mine was 0.61
float Kd = 9; //D Gain; Mine was 9
float Moving_Speed = 20; //Moving speed with Bluetooth Control; Mine was 20
float Max_Speed = 160; //Max mooving speed; Mine was 160
int Acc_Offset = 1045; //Accelerometer offset value (find this before you run the code)
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VARIABLES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
byte Activated, Received_byte;
int left_motor;
int Left_Motor_Speed;
int CC_Speed_Left_Motor;
int Left_Motor_Speed_Prev;
int right_motor;
int Right_Motor_Speed;
int CC_Speed_Right_Motor;
int Right_Motor_Speed_Prev;
int Received_Since;
int Gyro_X_Raw, Gyro_Y_Raw, Acc_Raw;
long Gyro_Y_Offset, Gyro_X_Offset;
unsigned long Loop_Time;
float Gyro_Angle, Acc_Angle, Auto_Setpoint;
float Temp_Error, PID_I, Setpoint, gyro_input, PID_Value, Last_D_Error;
float PID_Value_left, PID_Value_right;
int MPU6050_ADDR = 0x68; //MPU6050 I2C address (0x68 or sometimes 0x69)
/////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////Input/Output///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
int DIR_L = 2; //Pin for left driver direction pin
int STEP_L = 3; //Pin for left driver steps pin
int DIR_R = 4; //Pin for right driver direction pin
int STEP_R = 5; //Pin for right driver steps pin
int Enable = 6; //Pin for drivers enable (both use same pin)
int LED1 = 7; //Left LED is connected on D7
int LED2 = 8; //Right LED is connected on D8
int Buzzer = 9; //Buzzer is connected on D9
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////SETUP LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(9600); //Start the serial port at 9600 kbps
Wire.begin(); //Start I2C communication
TWBR = 12; //Also set I2C clock speed to 400kHz
//Define the pins mode (Output or Input)
pinMode(DIR_L, OUTPUT);
pinMode(STEP_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(STEP_R, OUTPUT);
pinMode(LED1, OUTPUT);
pinMode(LED2, OUTPUT);
pinMode(Buzzer, OUTPUT);
/*Read this: Now we define a "timer", timer 2 in this case. This timer will be used to create
the step pulses for the motors. This timer will click every 20us and make the required calculations.
The code will be inside the subroutine of TIMER2_COMPA_vect at the end of the code*/
TCCR2A = 0; //Start with TCCR2A set to zero
TCCR2B = 0; //Start with TCCR2B set to zero
TIMSK2 |= (1 << OCIE2A); //Interupt enable bit OCIE2A set to 1
TCCR2B |= (1 << CS21); //Set CS21 bit: We set prescaler to 8
OCR2A = 39; //Compare register is 39, so... 20us/(1s/(16MHz/8))-1
TCCR2A |= (1 << WGM21); //Mode: Clear timer on compare
//Start MPU6050 communication
Wire.beginTransmission(MPU6050_ADDR); //From the datastheet, the address is 0x68, but you can change that above.
Wire.write(0x6B); //Write on 0x6B register
Wire.write(0x00); //Set register to 00000000 and activate gyro
Wire.endTransmission(); //End the i2c transmission
//Change gyro scale to +/-250deg/sec
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1B); //Write on 0x1B register
Wire.write(0x00); //Set scale to 250dps, full scale
Wire.endTransmission(); //End the i2c transmission
//Change accelerometer scale to +/-4g.
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1C); //Write on 0x1C register
Wire.write(0x08); //Set scale to +/-4g
Wire.endTransmission(); //End the i2c transmission
//Enable some filters
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1A); //Write on 0x1A register
Wire.write(0x03); //Set Digital Low Pass Filter to ~43Hz
Wire.endTransmission(); //End the i2c transmission
/*When we start, the gyro might have an offset value. We make 520 readdings and get that calibration value
We use taht later in the code to substract the raw offset. */
for (int i = 0; i < 520; i++) { //Create 520 loops
if (i % 20 == 0){
digitalWrite(LED1, !digitalRead(LED1)); //Blink the LED every 20 loops
digitalWrite(Buzzer, !digitalRead(Buzzer)); //Buzz every 20 loops
}
Wire.beginTransmission(MPU6050_ADDR); //Start i2c communication with MPU6050
Wire.write(0x43); //We read from register 0x43
Wire.endTransmission(); //End the i2c transmission
Wire.requestFrom(MPU6050_ADDR, 4); //Request 2 bytes from the MPU6050
Gyro_Y_Offset += Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
Gyro_X_Offset += Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
delayMicroseconds(3500); //Small delay
}
Gyro_X_Offset /= 520; //Divide the total value by 520 to get the avarage gyro offset
Gyro_Y_Offset /= 520; //Divide the total value by 520 to get the avarage gyro offset
delay(200); //Small Delay
pinMode(Enable, OUTPUT); //Set Enable pin as OUTPUT
digitalWrite(Enable, LOW); //Finally, we enable the stepper drivers (drivers are enabled with LOW)
//Set the Loop_Time variable at the next end loop time
Loop_Time = micros() + 4000; //Loop time is 4000us
}
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VOID LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
if (Serial.available()) { //If there is serial data available from HC-05 module
Received_byte = Serial.read(); //Store the received data
Received_Since = 0; //Reset the counter
}
if (Received_Since <= 25) {
Received_Since ++; //The received data wwill last 25 loops, around 100 milliseconds
}
else Received_byte = 0x00; //After 100ms we reset the received data
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////CALCUALTE ANGLE/////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
Wire.beginTransmission(MPU6050_ADDR); //Start communication with MPU6050
Wire.write(0x3F); //Start reading register 3F
Wire.endTransmission(); //End i2c transmission
Wire.requestFrom(MPU6050_ADDR, 2); //Request 2 bytes from the MPU6050
Acc_Raw = Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
Acc_Raw += Acc_Offset; //Add the accelerometer offset value
if (Acc_Raw > 8200)Acc_Raw = 8200; //Prevent division by zero by limiting the acc data to +/-8200;
if (Acc_Raw < -8200)Acc_Raw = -8200; //Prevent division by zero by limiting the acc data to +/-8200;
Acc_Angle = asin((float)Acc_Raw / 8200.0) * 57.296; //Calculate the current angle according to the accelerometer data
if (Activated == 0 && Acc_Angle > -0.5 && Acc_Angle < 0.5) { //If the accelerometer angle is almost 0
Gyro_Angle = Acc_Angle; //Load the accelerometer angle in the Gyro_Angle variable
Activated = 1; //Set "Activated" variable and start PID control
}
Wire.beginTransmission(MPU6050_ADDR); //Start communication with MPU6050
Wire.write(0x43); //Start reading register 43
Wire.endTransmission(); //End i2c transmission
Wire.requestFrom(MPU6050_ADDR, 4); //Request 4 bytes from the gyro
Gyro_Y_Raw = Wire.read() << 8 | Wire.read(); ////Merge high and low byte and get an integer
Gyro_X_Raw = Wire.read() << 8 | Wire.read(); ////Merge high and low byte and get an integer
Gyro_X_Raw -= Gyro_X_Offset; //Add the gyro offset value
Gyro_Angle += Gyro_X_Raw * 0.000031; //Calculate traveled angle during this loop
Gyro_Y_Raw -= Gyro_Y_Offset; //Add gyro offset value
Gyro_Angle = Gyro_Angle * 0.9996 + Acc_Angle * 0.0004; //Correct the drift of the gyro angle with the accelerometer angle
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/*PID control si almos alwasy the same. We have a setpoint, which is the desired angle in this case (horizontal).
The PID will adjsut the speed and direction of the motors, so we can always go towards the desired angle. To get
the PID value, we use a PID algorithm formula.*/
//First, we calculate the error between the real angle and the value taht we want, in this case would be 0º
Temp_Error = Gyro_Angle - Auto_Setpoint - Setpoint;
if (PID_Value > 10 || PID_Value < -10) {
Temp_Error += PID_Value * 0.015 ;
}
//I value
PID_I += Ki * Temp_Error; //Calculate the "I" value
if (PID_I > 400)PID_I = 400; //We limit the "I" to the maximum output
else if (PID_I < -400)PID_I = -400;
//Calculate the PID output value
PID_Value = Kp * Temp_Error + PID_I + Kd * (Temp_Error - Last_D_Error);
if (PID_Value > 400)PID_Value = 400; //Limit the P+I to the maximum output
else if (PID_Value < -400)PID_Value = -400;
Last_D_Error = Temp_Error; //Store the error for the next loop
if (PID_Value < 6 && PID_Value > - 6)PID_Value = 0; //Dead-band where the robot is more or less balanced
if (Gyro_Angle > 30 || Gyro_Angle < -30 || Activated == 0) { //If the robot falls or the "Activated" is 0
PID_Value = 0; //Set the PID output to 0 so the motors are stopped
PID_I = 0; //Reset the I-controller memory
Activated = 0; //Set the Activated variable to 0
Auto_Setpoint = 0; //Reset the Auto_Setpoint variable
}
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////HC-05 CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
PID_Value_left = PID_Value; //Get PID output for the left motor
PID_Value_right = PID_Value; //Get PID output for the right motor
if (Received_byte & B00000001) { //We receive a a 00000001 so we turn Right
PID_Value_left += Moving_Speed; //Increase the left motor speed
PID_Value_right -= Moving_Speed; //Decrease the right motor speed
}
if (Received_byte & B00000010) { //We receive a a 00000010 so we turn Left
PID_Value_left -= Moving_Speed; //Decrease the left motor speed
PID_Value_right += Moving_Speed; //Increase the right motor speed
}
if (Received_byte & B00000100) { //We receive a a 00000100 so we go forward
if (Setpoint > -2.5)Setpoint -= 0.05; //Change the setpoint angle so the robot leans forwards
if (PID_Value > Max_Speed * -1)Setpoint -= 0.005; //Change the setpoint angle so the robot leans forwards
}
if (Received_byte & B00001000) { //We receive a a 00001000 so we go backwards
if (Setpoint < 2.5)Setpoint += 0.05; //Change the setpoint angle so the robot leans backwards
if (PID_Value < Max_Speed)Setpoint += 0.005; //Change the setpoint angle so the robot leans backwards
}
if (!(Received_byte & B00001100)) { //We receive a a 00001100 so no movement
if (Setpoint > 0.5)Setpoint -= 0.05; //If the PID setpoint is higher than 0.5, reduce setpoint by 0.05 every loop
else if (Setpoint < -0.5)Setpoint += 0.05; //If the PID setpoint is lower than -0.5, increase setpoint by 0.05 every loop
else Setpoint = 0; //If the PID setpoint is lower than 0.5 or highert than -0.5, set the setpoint to 0
}
if (Setpoint == 0) { //If the setpoint is zero degrees
if (PID_Value < 0)Auto_Setpoint += 0.001; //Increase the Auto_Setpoint if the robot is still moving forewards
if (PID_Value > 0)Auto_Setpoint -= 0.001; //Decrease the Auto_Setpoint if the robot is still moving backwards
}
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////MOTORS CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
if (PID_Value_left > 0){
PID_Value_left = 405 - (1 / (PID_Value_left + 9)) * 5500;
}
else if (PID_Value_left < 0){
PID_Value_left = -405 - (1 / (PID_Value_left - 9)) * 5500;
}
if (PID_Value_right > 0){
PID_Value_right = 405 - (1 / (PID_Value_right + 9)) * 5500;
}
else if (PID_Value_right < 0){
PID_Value_right = -405 - (1 / (PID_Value_right - 9)) * 5500;
}
//Calculate the pulse time for the left and right motor
if (PID_Value_left > 0){
left_motor = 400 - PID_Value_left;
}
else if (PID_Value_left < 0){
left_motor = -400 - PID_Value_left;
}
else left_motor = 0;
if (PID_Value_right > 0){
right_motor = 400 - PID_Value_right;
}
else if (PID_Value_right < 0){
right_motor = -400 - PID_Value_right;
}
else right_motor = 0;
Left_Motor_Speed = left_motor;
Right_Motor_Speed = right_motor;
/*The angle calculations are tuned for a loop time of 4 milliseconds.
//We make sure every loop is exactly 4 milliseconds so we set the Loop_Time to +4000 microseconds every loop.*/
while (Loop_Time > micros());
Loop_Time += 4000;
}//End of void loop
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////TIMER2_COMPA_vect////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect) {
//Left motor pulses
CC_Speed_Left_Motor ++; //Increase CC_Speed_Left_Motor by 1 every time this routine is executed
if (CC_Speed_Left_Motor > Left_Motor_Speed_Prev) { //If the number of loops is larger then the Left_Motor_Speed_Prev variable
CC_Speed_Left_Motor = 0; //Reset the CC_Speed_Left_Motor variable
Left_Motor_Speed_Prev = Left_Motor_Speed; //Load the next Left_Motor_Speed variable
if (Left_Motor_Speed_Prev < 0) { //If the Left_Motor_Speed_Prev is negative
PORTD &= 0b11111011; //Set D2 low. Reverse direction
Left_Motor_Speed_Prev *= -1; //Invert the Left_Motor_Speed_Prev variable
}
else PORTD |= 0b00000100; //Set output D2 high. Forward direction.
}
else if (CC_Speed_Left_Motor == 1)PORTD |= 0b00001000; //Set output D3 high to create a pulse for the stepper
else if (CC_Speed_Left_Motor == 2)PORTD &= 0b11110111; //Set output D3 low because the pulse only has to last for 20us
//Right motor pulses
CC_Speed_Right_Motor ++; //Increase CC_Speed_Right_Motor by 1 every time the routine is executed
if (CC_Speed_Right_Motor > Right_Motor_Speed_Prev) { //If the number of loops is larger then the Right_Motor_Speed_Prev variable
CC_Speed_Right_Motor = 0; //Reset the CC_Speed_Right_Motor variable
Right_Motor_Speed_Prev = Right_Motor_Speed; //Load the next Right_Motor_Speed variable
if (Right_Motor_Speed_Prev < 0) { //If the Right_Motor_Speed_Prev is negative
PORTD &= 0b11101111; //Set output D4 low. Reverse the direction
Right_Motor_Speed_Prev *= -1; //Invert the Right_Motor_Speed_Prev variable
}
else PORTD |= 0b00010000; //Set D4 high. Forward direction.
}
else if (CC_Speed_Right_Motor == 1)PORTD |= 0b00100000; //Set output D5 high to create a pulse for the stepper controller
else if (CC_Speed_Right_Motor == 2)PORTD &= 0b11011111; //Set output D5 low because the pulse only has to last for 20us
}//End of timer rroutine
