![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 |