/* 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
#include <Encoder.h>
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID VALUES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
float Kp = 28; //P Gain; Mine was 30
float Ki = 0.3; //I Gain; Mine was 0.61
float Kd = 4; //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 = 0.05; //Accelerometer offset value (find this before you run the code)
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
Encoder myEnc(11, 12);
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////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
}
long oldPosition = -999;
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VOID LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
// long Kd = myEnc.read()/8;
// if (Kd != oldPosition) {
// oldPosition = Kd;
// Serial.println(Kd);
// }
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 |= 0b00010000;
//Set
output D4 low. Reverse the direction
Right_Motor_Speed_Prev *= -1;
//Invert the Right_Motor_Speed_Prev variable
}
else PORTD &= 0b11101111;
//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