Lets get some code on this thing
So now we have the electronics sorted, lets have a look at the code. I’m borrowing heavily from this resource. I chose to use this code since the project was very well documented with some nice videos. The code also uses the same gyroscope as me. One downside is that this code is designed for stepper motors and I’m using cheap DC motors.
Before I jumped into the main code, I created a quick script just to check the motors were working. I created a few functions and ran them to ensure the motors moved correctly. If you look at this code you can also see how I am using the input pins of the motor driver to set direction and the enable pin with PWM to set the speed.
/* * Drive Motors with an Arduino Mini Pro using an L239D 'H' bridge * Keith Ellis * July 2018 * */ #define motor_left_A 2 // Motor Left Pin A - High = forward #define motor_left_B 4 // Motor Left Pin B - High = reverse #define motor_left_En 3 // Motor Left Enable Pin, PWM on this pin will control speed #define motor_right_A 5 // Motor Right Pin A - High = forward #define motor_right_B 7 // Motor Right Pin B - High = reverse #define motor_right_En 6 // Motor Right Enable Pin, PWM on this pin will control speed void setup() { pinMode(motor_left_A, OUTPUT); pinMode(motor_left_B, OUTPUT); pinMode(motor_left_En, OUTPUT); pinMode(motor_right_A, OUTPUT); pinMode(motor_right_B, OUTPUT); pinMode(motor_right_En, OUTPUT); } void loop() { AllMotorsFwd(127); // Speed from 0 - 255 delay(100); AllMotorsRev(255); delay(75); AllMotorsFwd(127); delay(100); AllMotorsRev(255); delay(120); AllMotorsFwd(127); delay(50); AllMotorsRev(255); delay(75); AllMotorsFwd(127); delay(100); AllMotorsRev(127); delay(125); } void LeftMotorFwd(int speed){ //Speed is int from 0 - 255 digitalWrite(motor_left_A, HIGH); digitalWrite(motor_left_B, LOW); analogWrite(motor_left_En, speed); } void LeftMotorRev(int speed){ //Speed is int from 0 - 255 digitalWrite(motor_left_A, LOW); digitalWrite(motor_left_B, HIGH); analogWrite(motor_left_En, speed); } void RightMotorFwd(int speed){ //Speed is int from 0 - 255 digitalWrite(motor_right_A, HIGH); digitalWrite(motor_right_B, LOW); analogWrite(motor_right_En, speed); } void RightMotorRev(int speed){ //Speed is int from 0 - 255 digitalWrite(motor_right_A, LOW); digitalWrite(motor_right_B, HIGH); analogWrite(motor_right_En, speed); } void AllMotorsFwd(int speed){ //Speed is int from 0 - 255 LeftMotorFwd(speed); RightMotorFwd(speed); } void AllMotorsRev(int speed){ //Speed is int from 0 - 255 LeftMotorRev(speed); RightMotorRev(speed); } void LeftMotorStop(void){ digitalWrite(motor_left_A, LOW); digitalWrite(motor_left_B, LOW); digitalWrite(motor_left_En, LOW); } void RightMotorStop(void){ digitalWrite(motor_right_A, LOW); digitalWrite(motor_right_B, LOW); digitalWrite(motor_right_En, LOW); } void AllMotorsStop(void){ LeftMotorStop(); RightMotorStop(); }
This worked as planned, you can download this code here
After editing the Balance Bot code to take out references to the stepped motors and copying in my motor functions I ended up with the code here.
Before running this code, the balance point of the robot needs to be determined. This can be done by running the calibration code here Balnce the robot upright and open the serial console. Make a note of the Balance value and paste it into the variable acc_calibration_value on line 17 of the main Balance_robot.ino code.
After messing about with the PID variables I managed to get the robot balancing of sorts as seen above. To tune the PID values I used this video here. I was well chuffed, but it is far from perfect. I’ll leave this post here for now, my next post will run through PID tuning and weight placement.