Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_1

Prototype program for balancing robot based on various examples from other users.

Dependencies:   HCSR04 MPU6050_1 Motor Servo ledControl2 mbed

main.cpp@0:cfae0986065f, 2016-08-26 (annotated)

Committer:
lakshmananag
Date:
Fri Aug 26 07:11:47 2016 +0000
Revision:
0:cfae0986065f
Prototype program for balancing robot.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lakshmananag 0:cfae0986065f 1 #include "mbed.h"
lakshmananag 0:cfae0986065f 2 #include "hcsr04.h"
lakshmananag 0:cfae0986065f 3 #include "MPU6050.h"
lakshmananag 0:cfae0986065f 4 #include "ledControl.h"
lakshmananag 0:cfae0986065f 5 #include "Motor.h"
lakshmananag 0:cfae0986065f 6 #include "HALLFX_ENCODER.h"
lakshmananag 0:cfae0986065f 7
lakshmananag 0:cfae0986065f 8 Serial pc(USBTX, USBRX); // Create terminal link
lakshmananag 0:cfae0986065f 9 Serial blue(p28, p27); // Bluetooth TX, RX
lakshmananag 0:cfae0986065f 10
lakshmananag 0:cfae0986065f 11 MPU6050 mpu6050; // mpu6050 object from MPU6050 classs
lakshmananag 0:cfae0986065f 12
lakshmananag 0:cfae0986065f 13 Ticker toggler1; // Ticker for led toggling
lakshmananag 0:cfae0986065f 14 Ticker filter; // Ticker for periodic call to compFilter funcçs
lakshmananag 0:cfae0986065f 15 Ticker balance; // Periodic routine for PID control of balance system
lakshmananag 0:cfae0986065f 16 Ticker speed; // Periodic routine for speed control
lakshmananag 0:cfae0986065f 17 Ticker bluetooth; // Ticker for navigation
lakshmananag 0:cfae0986065f 18
lakshmananag 0:cfae0986065f 19 Motor A(p23, p5, p6); // pwm, fwd, rev
lakshmananag 0:cfae0986065f 20 Motor B(p24, p7, p8); // pwm, fwd, rev
lakshmananag 0:cfae0986065f 21
lakshmananag 0:cfae0986065f 22 /* Encoders*/
lakshmananag 0:cfae0986065f 23 HALLFX_ENCODER leftEncoder(p13);
lakshmananag 0:cfae0986065f 24 HALLFX_ENCODER rightEncoder(p14);
lakshmananag 0:cfae0986065f 25
lakshmananag 0:cfae0986065f 26 HCSR04 usensor(p25,p26); //trig pin,echo pin
lakshmananag 0:cfae0986065f 27
lakshmananag 0:cfae0986065f 28 /* Function prototypes */
lakshmananag 0:cfae0986065f 29 void toggle_led1();
lakshmananag 0:cfae0986065f 30 void toggle_led2();
lakshmananag 0:cfae0986065f 31 void compFilter();
lakshmananag 0:cfae0986065f 32 void balancePID();
lakshmananag 0:cfae0986065f 33 void balanceControl();
lakshmananag 0:cfae0986065f 34 void Forward(float);
lakshmananag 0:cfae0986065f 35 void Reverse(float);
lakshmananag 0:cfae0986065f 36 void Stop(void);
lakshmananag 0:cfae0986065f 37 void Navigate();
lakshmananag 0:cfae0986065f 38 void TurnRight(float);
lakshmananag 0:cfae0986065f 39 void TurnLeft(float);
lakshmananag 0:cfae0986065f 40
lakshmananag 0:cfae0986065f 41 /* Variable declarations */
lakshmananag 0:cfae0986065f 42 float pitchAngle = 0;
lakshmananag 0:cfae0986065f 43 float rollAngle = 0;
lakshmananag 0:cfae0986065f 44 bool dir; // direction of movement
lakshmananag 0:cfae0986065f 45 float Kp = 0.5;
lakshmananag 0:cfae0986065f 46 float Ki = 0.00001;
lakshmananag 0:cfae0986065f 47 float Kd = 0.01;//0.01;//0.05;
lakshmananag 0:cfae0986065f 48 float set_point = 1.005; // Angle for staying upright
lakshmananag 0:cfae0986065f 49 float errorPID = 0; // Proportional term (P) in PID
lakshmananag 0:cfae0986065f 50 float last_errorPID =0; // Previous error value
lakshmananag 0:cfae0986065f 51 float integral = 0; // Integral (I)
lakshmananag 0:cfae0986065f 52 float derivative = 0; // Derivative (D)
lakshmananag 0:cfae0986065f 53 float outputPID = 0; // Output of PID calculations
lakshmananag 0:cfae0986065f 54 float position = 0.0;
lakshmananag 0:cfae0986065f 55 float dist=0.0;
lakshmananag 0:cfae0986065f 56 float range = 0.000;
lakshmananag 0:cfae0986065f 57 int phone_char;
lakshmananag 0:cfae0986065f 58 DigitalOut myled(LED4);
lakshmananag 0:cfae0986065f 59
lakshmananag 0:cfae0986065f 60 int main()
lakshmananag 0:cfae0986065f 61 {
lakshmananag 0:cfae0986065f 62 pc.baud(9600); // baud rate: 9600
lakshmananag 0:cfae0986065f 63 blue.baud(9600);
lakshmananag 0:cfae0986065f 64
lakshmananag 0:cfae0986065f 65 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
lakshmananag 0:cfae0986065f 66 wait(0.5);
lakshmananag 0:cfae0986065f 67 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
lakshmananag 0:cfae0986065f 68 pc.printf("Calibration is completed. \r\n");
lakshmananag 0:cfae0986065f 69 mpu6050.init(); // Initialize the sensor
lakshmananag 0:cfae0986065f 70 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
lakshmananag 0:cfae0986065f 71
lakshmananag 0:cfae0986065f 72 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
lakshmananag 0:cfae0986065f 73 balance.attach(&balancePID, 0.010); // Same period with balancePID
lakshmananag 0:cfae0986065f 74 speed.attach(&balanceControl, 0.01);
lakshmananag 0:cfae0986065f 75 bluetooth.attach(&Navigate, 0.05);
lakshmananag 0:cfae0986065f 76
lakshmananag 0:cfae0986065f 77 while(1)
lakshmananag 0:cfae0986065f 78 {
lakshmananag 0:cfae0986065f 79
lakshmananag 0:cfae0986065f 80 if(blue.readable())
lakshmananag 0:cfae0986065f 81 {
lakshmananag 0:cfae0986065f 82 phone_char = blue.getc();
lakshmananag 0:cfae0986065f 83 pc.putc(phone_char);
lakshmananag 0:cfae0986065f 84 pc.printf("Bluetooth Start\r\n");
lakshmananag 0:cfae0986065f 85 myled = !myled;
lakshmananag 0:cfae0986065f 86 }
lakshmananag 0:cfae0986065f 87
lakshmananag 0:cfae0986065f 88 pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f\r\n",rollAngle,pitchAngle);
lakshmananag 0:cfae0986065f 89 pc.printf("Error = %f\r\n",outputPID);
lakshmananag 0:cfae0986065f 90 wait_ms(40);
lakshmananag 0:cfae0986065f 91 }
lakshmananag 0:cfae0986065f 92 }
lakshmananag 0:cfae0986065f 93
lakshmananag 0:cfae0986065f 94 void toggle_led1() {ledToggle(1);}
lakshmananag 0:cfae0986065f 95 void toggle_led2() {ledToggle(2);}
lakshmananag 0:cfae0986065f 96
lakshmananag 0:cfae0986065f 97 /* This function is calls the complementary filter with pitch and roll angles */
lakshmananag 0:cfae0986065f 98 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
lakshmananag 0:cfae0986065f 99
lakshmananag 0:cfae0986065f 100 void balancePID()
lakshmananag 0:cfae0986065f 101 {
lakshmananag 0:cfae0986065f 102 errorPID = set_point - pitchAngle; //Proportional (P)
lakshmananag 0:cfae0986065f 103 integral += errorPID; //Integral (I)
lakshmananag 0:cfae0986065f 104 derivative = errorPID - last_errorPID; //Derivative (D)
lakshmananag 0:cfae0986065f 105
lakshmananag 0:cfae0986065f 106 last_errorPID = errorPID; //Save current value for next iteration
lakshmananag 0:cfae0986065f 107
lakshmananag 0:cfae0986065f 108 outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative); //PID calculation
lakshmananag 0:cfae0986065f 109
lakshmananag 0:cfae0986065f 110 /* errorPID is restricted between -1 and 1 */
lakshmananag 0:cfae0986065f 111 if(outputPID > 0.8)
lakshmananag 0:cfae0986065f 112 outputPID = 0.8;
lakshmananag 0:cfae0986065f 113 else if(outputPID < -0.8)
lakshmananag 0:cfae0986065f 114 outputPID = -0.8;
lakshmananag 0:cfae0986065f 115 }
lakshmananag 0:cfae0986065f 116
lakshmananag 0:cfae0986065f 117 void balanceControl()
lakshmananag 0:cfae0986065f 118 {
lakshmananag 0:cfae0986065f 119 int direction=0; // Variable to hold direction we want to drive
lakshmananag 0:cfae0986065f 120 if (outputPID>0)direction=1; // Positive speed indicates forward
lakshmananag 0:cfae0986065f 121 if (outputPID<0)direction=2; // Negative speed indicates backwards
lakshmananag 0:cfae0986065f 122 if((abs(pitchAngle)>10.00))direction=0;
lakshmananag 0:cfae0986065f 123
lakshmananag 0:cfae0986065f 124 switch( direction) { // Depending on what direction was passed
lakshmananag 0:cfae0986065f 125 case 0: // Stop case
lakshmananag 0:cfae0986065f 126 Stop();
lakshmananag 0:cfae0986065f 127 break;
lakshmananag 0:cfae0986065f 128 case 1: // Forward case
lakshmananag 0:cfae0986065f 129 Forward(abs(outputPID));
lakshmananag 0:cfae0986065f 130 break;
lakshmananag 0:cfae0986065f 131 case 2: // Backwards
lakshmananag 0:cfae0986065f 132 Reverse(abs(outputPID));
lakshmananag 0:cfae0986065f 133 break;
lakshmananag 0:cfae0986065f 134 default: // Catch-all (Stop)
lakshmananag 0:cfae0986065f 135 Stop();
lakshmananag 0:cfae0986065f 136 break;
lakshmananag 0:cfae0986065f 137 }
lakshmananag 0:cfae0986065f 138 }
lakshmananag 0:cfae0986065f 139
lakshmananag 0:cfae0986065f 140 void Stop(void)
lakshmananag 0:cfae0986065f 141 {
lakshmananag 0:cfae0986065f 142 A.speed(0.0);
lakshmananag 0:cfae0986065f 143 B.speed(0.0);
lakshmananag 0:cfae0986065f 144 }
lakshmananag 0:cfae0986065f 145
lakshmananag 0:cfae0986065f 146 void Forward(float fwd_dist)
lakshmananag 0:cfae0986065f 147 {
lakshmananag 0:cfae0986065f 148 A.speed(-fwd_dist);
lakshmananag 0:cfae0986065f 149 B.speed(-fwd_dist);
lakshmananag 0:cfae0986065f 150 }
lakshmananag 0:cfae0986065f 151
lakshmananag 0:cfae0986065f 152 void TurnRight(float ryt_dist)
lakshmananag 0:cfae0986065f 153 {
lakshmananag 0:cfae0986065f 154 A.speed(ryt_dist);
lakshmananag 0:cfae0986065f 155 B.speed(-ryt_dist);
lakshmananag 0:cfae0986065f 156 }
lakshmananag 0:cfae0986065f 157
lakshmananag 0:cfae0986065f 158 void TurnLeft(float lft_dist)
lakshmananag 0:cfae0986065f 159 {
lakshmananag 0:cfae0986065f 160 A.speed(-lft_dist);
lakshmananag 0:cfae0986065f 161 B.speed(lft_dist);
lakshmananag 0:cfae0986065f 162 }
lakshmananag 0:cfae0986065f 163
lakshmananag 0:cfae0986065f 164 void Reverse(float rev_dist)
lakshmananag 0:cfae0986065f 165 {
lakshmananag 0:cfae0986065f 166 A.speed(rev_dist);
lakshmananag 0:cfae0986065f 167 B.speed(rev_dist);
lakshmananag 0:cfae0986065f 168 }
lakshmananag 0:cfae0986065f 169
lakshmananag 0:cfae0986065f 170 void Navigate()
lakshmananag 0:cfae0986065f 171 {
lakshmananag 0:cfae0986065f 172 switch (phone_char)
lakshmananag 0:cfae0986065f 173 {
lakshmananag 0:cfae0986065f 174 case 'f':
lakshmananag 0:cfae0986065f 175 Forward(0.7);
lakshmananag 0:cfae0986065f 176 break;
lakshmananag 0:cfae0986065f 177
lakshmananag 0:cfae0986065f 178 case 'F':
lakshmananag 0:cfae0986065f 179 Forward(0.7);
lakshmananag 0:cfae0986065f 180 break;
lakshmananag 0:cfae0986065f 181
lakshmananag 0:cfae0986065f 182 case 'b':
lakshmananag 0:cfae0986065f 183 Reverse(0.7);
lakshmananag 0:cfae0986065f 184 break;
lakshmananag 0:cfae0986065f 185
lakshmananag 0:cfae0986065f 186 case 'B':
lakshmananag 0:cfae0986065f 187 Reverse(0.7);
lakshmananag 0:cfae0986065f 188 break;
lakshmananag 0:cfae0986065f 189
lakshmananag 0:cfae0986065f 190 case 'r':
lakshmananag 0:cfae0986065f 191 TurnRight(0.7);
lakshmananag 0:cfae0986065f 192 break;
lakshmananag 0:cfae0986065f 193
lakshmananag 0:cfae0986065f 194 case 'R':
lakshmananag 0:cfae0986065f 195 TurnRight(0.7);
lakshmananag 0:cfae0986065f 196 break;
lakshmananag 0:cfae0986065f 197
lakshmananag 0:cfae0986065f 198 case 'l':
lakshmananag 0:cfae0986065f 199 TurnLeft(0.7);
lakshmananag 0:cfae0986065f 200 break;
lakshmananag 0:cfae0986065f 201
lakshmananag 0:cfae0986065f 202 case 'L':
lakshmananag 0:cfae0986065f 203 TurnLeft(0.7);
lakshmananag 0:cfae0986065f 204 break;
lakshmananag 0:cfae0986065f 205
lakshmananag 0:cfae0986065f 206 default:
lakshmananag 0:cfae0986065f 207 Stop();
lakshmananag 0:cfae0986065f 208 }
lakshmananag 0:cfae0986065f 209 }
lakshmananag 0:cfae0986065f 210