zejun du / Mbed 2 deprecated inverted_pendulum_system

this code is for inverted pendulum balancer. it is based on two-loop PID controller. One controller is for Angle and another one is for velocity.

Dependencies:   HCSR04 L298HBridge mbed

communication/communication.cpp@1:ce626b794a6c, 2017-08-25 (annotated)

Committer:
dudu941014
Date:
Fri Aug 25 21:25:46 2017 +0000
Revision:
1:ce626b794a6c
Parent:
0:489498e8dae5 
this code is for inverted pendulum balancer. it is based on two-loop PID controller. One controller is for Angle and another one is for velocity.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dudu941014 0:489498e8dae5 1 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 2 // Company: edinburgh of university
dudu941014 0:489498e8dae5 3 // Engineer: ZEjun DU
dudu941014 0:489498e8dae5 4 //
dudu941014 0:489498e8dae5 5 // Create Date: 2017/08/20 13:06:52
dudu941014 0:489498e8dae5 6 // Design Name: Inverted Pendulum Balancer
dudu941014 0:489498e8dae5 7 // Module Name: commnication with GUI
dudu941014 0:489498e8dae5 8 // Tool Versions: “Keil 5” or “Mbed Complie Online”
dudu941014 0:489498e8dae5 9 // Description: this funcition is to commucation with GUI through Bluetooth HC05
dudu941014 0:489498e8dae5 10 // the system can invoke this function to send the packet and decode the result
dudu941014 0:489498e8dae5 11 // Pin 28 --------> (TX) RX pin of the Bluetooth module
dudu941014 0:489498e8dae5 12 // Pin 27 --------> (RX) TX pin of the Bluetooth module
dudu941014 0:489498e8dae5 13 // GND -----------> GND of the Bluetooth module
dudu941014 0:489498e8dae5 14 // VOUT (3.3 V) --> VCC of the Bluetooth module
dudu941014 0:489498e8dae5 15 //
dudu941014 0:489498e8dae5 16 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 17
dudu941014 0:489498e8dae5 18 #include "communication.h"
dudu941014 0:489498e8dae5 19 #include "get_angle.h"
dudu941014 0:489498e8dae5 20 #include "controller.h"
dudu941014 0:489498e8dae5 21 #include "stabilizer.h"
dudu941014 0:489498e8dae5 22 #include "distance.h"
dudu941014 0:489498e8dae5 23
dudu941014 0:489498e8dae5 24 Serial bluetooth(p28,p27); // p28: TX, p27: RX (LPC1768)
dudu941014 0:489498e8dae5 25
dudu941014 0:489498e8dae5 26 DigitalOut led1(LED1);//LED init
dudu941014 0:489498e8dae5 27 DigitalOut led2(LED2);//LED init
dudu941014 0:489498e8dae5 28 DigitalOut led3(LED3);//LED init
dudu941014 0:489498e8dae5 29 DigitalOut led4(LED4);//LED init
dudu941014 0:489498e8dae5 30
dudu941014 0:489498e8dae5 31 int j = 500;//the defualt PWM(50% PWM)
dudu941014 0:489498e8dae5 32 int i = 0; //the Bytes' numbers
dudu941014 0:489498e8dae5 33 float AY, EP, ER;//redundant variables
dudu941014 0:489498e8dae5 34
dudu941014 0:489498e8dae5 35 uint8_t usart_rx_buf[64];//this variable is to load the packet from GUI
dudu941014 0:489498e8dae5 36
dudu941014 0:489498e8dae5 37 uint8_t Tx_Buf[24] = //this is the buff which should be sent to GUI
dudu941014 0:489498e8dae5 38 {
dudu941014 0:489498e8dae5 39 'A','T',
dudu941014 0:489498e8dae5 40 '0','0','0','0',
dudu941014 0:489498e8dae5 41 '0','0','0','0',
dudu941014 0:489498e8dae5 42 '0','0','0','0',
dudu941014 0:489498e8dae5 43 '0','0','0','0',
dudu941014 0:489498e8dae5 44 '0','0','0','0',
dudu941014 0:489498e8dae5 45 '0','0'
dudu941014 0:489498e8dae5 46 };
dudu941014 0:489498e8dae5 47
dudu941014 0:489498e8dae5 48
dudu941014 0:489498e8dae5 49 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 50 //this funcition is to load the data (distance and angle) and send the whole packet
dudu941014 0:489498e8dae5 51 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 52 void Send_Messge(void)
dudu941014 0:489498e8dae5 53 {
dudu941014 0:489498e8dae5 54 int X;//the number of current Byte
dudu941014 0:489498e8dae5 55
dudu941014 0:489498e8dae5 56 Tx_Buf[2] = (int32_t)angle>>24;//send 8 bits each time
dudu941014 0:489498e8dae5 57 Tx_Buf[3] = (int32_t)angle>>16;//angle
dudu941014 0:489498e8dae5 58 Tx_Buf[4] = (int32_t)angle>>8;
dudu941014 0:489498e8dae5 59 Tx_Buf[5] = (int32_t)angle;
dudu941014 0:489498e8dae5 60
dudu941014 0:489498e8dae5 61 Tx_Buf[6] = (int32_t)distance>>24;//send 8 bits each time
dudu941014 0:489498e8dae5 62 Tx_Buf[7] = (int32_t)distance>>16;//distance
dudu941014 0:489498e8dae5 63 Tx_Buf[8] = (int32_t)distance>>8;
dudu941014 0:489498e8dae5 64 Tx_Buf[9] = (int32_t)distance;
dudu941014 0:489498e8dae5 65
dudu941014 0:489498e8dae5 66 Tx_Buf[10] = (int32_t)AY>>24;//send 8 bits each time
dudu941014 0:489498e8dae5 67 Tx_Buf[11] = (int32_t)AY>>16;
dudu941014 0:489498e8dae5 68 Tx_Buf[12] = (int32_t)AY>>8;
dudu941014 0:489498e8dae5 69 Tx_Buf[13] = (int32_t)AY;
dudu941014 0:489498e8dae5 70
dudu941014 0:489498e8dae5 71 Tx_Buf[14] = (int32_t)EP>>24;//send 8 bits each time
dudu941014 0:489498e8dae5 72 Tx_Buf[15] = (int32_t)EP>>16;
dudu941014 0:489498e8dae5 73 Tx_Buf[16] = (int32_t)EP>>8;
dudu941014 0:489498e8dae5 74 Tx_Buf[17] = (int32_t)EP;
dudu941014 0:489498e8dae5 75
dudu941014 0:489498e8dae5 76 Tx_Buf[18] = (int32_t)ER>>24;//send 8 bits each time
dudu941014 0:489498e8dae5 77 Tx_Buf[19] = (int32_t)ER>>16;
dudu941014 0:489498e8dae5 78 Tx_Buf[20] = (int32_t)ER>>8;
dudu941014 0:489498e8dae5 79 Tx_Buf[21] = (int32_t)ER;
dudu941014 0:489498e8dae5 80
dudu941014 0:489498e8dae5 81 Tx_Buf[22] = 0;
dudu941014 0:489498e8dae5 82 Tx_Buf[23] = 0;
dudu941014 0:489498e8dae5 83 // bluetooth.printf("this sentence is used to test");
dudu941014 0:489498e8dae5 84
dudu941014 0:489498e8dae5 85 for(X=0; X<24; X++)
dudu941014 0:489498e8dae5 86 {
dudu941014 0:489498e8dae5 87 bluetooth.printf("%c", Tx_Buf[X]);//send the packet
dudu941014 0:489498e8dae5 88 }
dudu941014 0:489498e8dae5 89 }
dudu941014 0:489498e8dae5 90
dudu941014 0:489498e8dae5 91
dudu941014 0:489498e8dae5 92 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 93 //this funcition is to deocode the data from GUI
dudu941014 0:489498e8dae5 94 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 95 void commander(void)
dudu941014 0:489498e8dae5 96 {
dudu941014 0:489498e8dae5 97 usart_rx_buf[i] = bluetooth.getc();
dudu941014 0:489498e8dae5 98 i++;
dudu941014 0:489498e8dae5 99 if(i==6){
dudu941014 0:489498e8dae5 100 i=0;
dudu941014 0:489498e8dae5 101 if( (usart_rx_buf[0]=='G') && (usart_rx_buf[1]=='O'))//"GO"
dudu941014 0:489498e8dae5 102 {
dudu941014 0:489498e8dae5 103 PID_ctrl = 1;//enable PID mode
dudu941014 0:489498e8dae5 104 Velocity_ctrl = 1;//enable velocity PID
dudu941014 0:489498e8dae5 105
dudu941014 0:489498e8dae5 106 led1 = 1;
dudu941014 0:489498e8dae5 107 led2 = 0;
dudu941014 0:489498e8dae5 108 led3 = 0;
dudu941014 0:489498e8dae5 109 led4 = 0;
dudu941014 0:489498e8dae5 110 }//unlock the inverted pendulum, its means that we start the pid control
dudu941014 0:489498e8dae5 111
dudu941014 0:489498e8dae5 112 if( (usart_rx_buf[0]=='S') && (usart_rx_buf[1]=='T'))//"STOP"
dudu941014 0:489498e8dae5 113 {
dudu941014 0:489498e8dae5 114 PID_ctrl = 0;//disable PID mode
dudu941014 0:489498e8dae5 115 Velocity_ctrl = 0;//disable velocity PID
dudu941014 0:489498e8dae5 116
dudu941014 0:489498e8dae5 117 led1 = 0;
dudu941014 0:489498e8dae5 118 led2 = 1;
dudu941014 0:489498e8dae5 119 led3 = 0;
dudu941014 0:489498e8dae5 120 led4 = 0;
dudu941014 0:489498e8dae5 121 MOTOR_Update(0);
dudu941014 0:489498e8dae5 122 }//shut down all controlling
dudu941014 0:489498e8dae5 123
dudu941014 0:489498e8dae5 124 if( (usart_rx_buf[0]=='A') && (usart_rx_buf[1]=='U'))//keyboard control
dudu941014 0:489498e8dae5 125 {
dudu941014 0:489498e8dae5 126 if(usart_rx_buf[2]=='1'){//go forward manually
dudu941014 0:489498e8dae5 127 Motor_PWM.Stop();//to avoid turnning on all switches of L298n. turnning on all switches will damage the L298n
dudu941014 0:489498e8dae5 128 wait_us(50);
dudu941014 0:489498e8dae5 129 Motor_PWM.Fwd();
dudu941014 0:489498e8dae5 130 Motor_PWM.Speed(j);
dudu941014 0:489498e8dae5 131 led1 = 0;
dudu941014 0:489498e8dae5 132 led2 = 0;
dudu941014 0:489498e8dae5 133 led3 = 1;
dudu941014 0:489498e8dae5 134 led4 = 0;
dudu941014 0:489498e8dae5 135
dudu941014 0:489498e8dae5 136 }
dudu941014 0:489498e8dae5 137 else if(usart_rx_buf[2]=='2'){//go reverse manually
dudu941014 0:489498e8dae5 138 Motor_PWM.Stop();
dudu941014 0:489498e8dae5 139 wait_us(50);
dudu941014 0:489498e8dae5 140 Motor_PWM.Rev();
dudu941014 0:489498e8dae5 141 Motor_PWM.Speed(j);
dudu941014 0:489498e8dae5 142 led1 = 0;
dudu941014 0:489498e8dae5 143 led2 = 0;
dudu941014 0:489498e8dae5 144 led3 = 0;
dudu941014 0:489498e8dae5 145 led4 = 1;
dudu941014 0:489498e8dae5 146 }
dudu941014 0:489498e8dae5 147 else if(usart_rx_buf[2]=='3'){//change the force to motor manually
dudu941014 0:489498e8dae5 148 j=j+100;
dudu941014 0:489498e8dae5 149 if(j>=1000)
dudu941014 0:489498e8dae5 150 j=1000;
dudu941014 0:489498e8dae5 151 Motor_PWM.Speed(j);
dudu941014 0:489498e8dae5 152 led1 = 0;
dudu941014 0:489498e8dae5 153 led2 = 0;
dudu941014 0:489498e8dae5 154 led3 = 1;
dudu941014 0:489498e8dae5 155 led4 = 0;
dudu941014 0:489498e8dae5 156 }
dudu941014 0:489498e8dae5 157 else if(usart_rx_buf[2]=='4'){//change the force to motor manually
dudu941014 0:489498e8dae5 158 j=j-100;
dudu941014 0:489498e8dae5 159 if(j<=0)
dudu941014 0:489498e8dae5 160 j=0;
dudu941014 0:489498e8dae5 161 Motor_PWM.Speed(j);
dudu941014 0:489498e8dae5 162 led1 = 0;
dudu941014 0:489498e8dae5 163 led2 = 0;
dudu941014 0:489498e8dae5 164 led3 = 0;
dudu941014 0:489498e8dae5 165 led4 = 1;
dudu941014 0:489498e8dae5 166 }
dudu941014 0:489498e8dae5 167 else if(usart_rx_buf[2]=='5'){
dudu941014 0:489498e8dae5 168 PID_ctrl = 0;
dudu941014 0:489498e8dae5 169
dudu941014 0:489498e8dae5 170 led1 = 1;
dudu941014 0:489498e8dae5 171 led2 = 1;
dudu941014 0:489498e8dae5 172 led3 = 0;
dudu941014 0:489498e8dae5 173 led4 = 0;
dudu941014 0:489498e8dae5 174 MOTOR_Update(0);
dudu941014 0:489498e8dae5 175 }
dudu941014 0:489498e8dae5 176
dudu941014 0:489498e8dae5 177 else if(usart_rx_buf[2]=='6'){//set the original of angle and distance
dudu941014 0:489498e8dae5 178 angle_offset = angle_voltage.read();
dudu941014 0:489498e8dae5 179 usensor.start();
dudu941014 0:489498e8dae5 180 home = usensor.get_dist_cm();
dudu941014 0:489498e8dae5 181
dudu941014 0:489498e8dae5 182 led1 = 1;
dudu941014 0:489498e8dae5 183 led2 = 1;
dudu941014 0:489498e8dae5 184 led3 = 1;
dudu941014 0:489498e8dae5 185 led4 = 1;
dudu941014 0:489498e8dae5 186
dudu941014 0:489498e8dae5 187 }
dudu941014 0:489498e8dae5 188 else if(usart_rx_buf[2]=='7'){//set the original of distance
dudu941014 0:489498e8dae5 189 usensor.start();
dudu941014 0:489498e8dae5 190 home = usensor.get_dist_cm();
dudu941014 0:489498e8dae5 191 Velocity_ctrl = 1;
dudu941014 0:489498e8dae5 192
dudu941014 0:489498e8dae5 193 led1 = 1;
dudu941014 0:489498e8dae5 194 led2 = 0;
dudu941014 0:489498e8dae5 195 led3 = 1;
dudu941014 0:489498e8dae5 196 led4 = 0;
dudu941014 0:489498e8dae5 197
dudu941014 0:489498e8dae5 198 }
dudu941014 0:489498e8dae5 199 }// change the direction of car by keyboard
dudu941014 0:489498e8dae5 200
dudu941014 0:489498e8dae5 201
dudu941014 0:489498e8dae5 202
dudu941014 0:489498e8dae5 203
dudu941014 0:489498e8dae5 204 //change the volecity of car
dudu941014 0:489498e8dae5 205 if(usart_rx_buf[0] == 'P' && usart_rx_buf[1] == 'W')
dudu941014 0:489498e8dae5 206 {
dudu941014 0:489498e8dae5 207 M_Thr = (usart_rx_buf[2] << 8) | usart_rx_buf[3];
dudu941014 0:489498e8dae5 208 MOTOR_Update(M_Thr);
dudu941014 0:489498e8dae5 209 }
dudu941014 0:489498e8dae5 210
dudu941014 0:489498e8dae5 211 //Launch Control
dudu941014 0:489498e8dae5 212 if(usart_rx_buf[0] == 'L' && usart_rx_buf[1] == 'C')
dudu941014 0:489498e8dae5 213 {
dudu941014 0:489498e8dae5 214 PID_ctrl = 0;
dudu941014 0:489498e8dae5 215 Velocity_ctrl = 0;
dudu941014 0:489498e8dae5 216 if(angle <=0)
dudu941014 0:489498e8dae5 217 {
dudu941014 0:489498e8dae5 218 Motor_PWM.Rev();
dudu941014 0:489498e8dae5 219 Motor_PWM.Speed(900);
dudu941014 0:489498e8dae5 220 wait(0.7);
dudu941014 0:489498e8dae5 221 Motor_PWM.Fwd();
dudu941014 0:489498e8dae5 222 Motor_PWM.Speed(700);
dudu941014 0:489498e8dae5 223 wait(0.3);
dudu941014 0:489498e8dae5 224 }
dudu941014 0:489498e8dae5 225 else
dudu941014 0:489498e8dae5 226 {
dudu941014 0:489498e8dae5 227 Motor_PWM.Fwd();
dudu941014 0:489498e8dae5 228 Motor_PWM.Speed(900);
dudu941014 0:489498e8dae5 229 wait(0.7);
dudu941014 0:489498e8dae5 230 Motor_PWM.Rev();
dudu941014 0:489498e8dae5 231 Motor_PWM.Speed(700);
dudu941014 0:489498e8dae5 232 wait(0.3);
dudu941014 0:489498e8dae5 233 }
dudu941014 0:489498e8dae5 234 PID_ctrl = 1;
dudu941014 0:489498e8dae5 235 Velocity_ctrl = 1;
dudu941014 0:489498e8dae5 236
dudu941014 0:489498e8dae5 237 }
dudu941014 0:489498e8dae5 238
dudu941014 0:489498e8dae5 239
dudu941014 0:489498e8dae5 240
dudu941014 0:489498e8dae5 241 //this is function for distance
dudu941014 0:489498e8dae5 242 //to set the 'home' point
dudu941014 0:489498e8dae5 243 if( (usart_rx_buf[0]=='H') && (usart_rx_buf[1]=='O'))
dudu941014 0:489498e8dae5 244 {
dudu941014 0:489498e8dae5 245 usensor.start();
dudu941014 0:489498e8dae5 246 home = usensor.get_dist_cm();
dudu941014 0:489498e8dae5 247 }
dudu941014 0:489498e8dae5 248
dudu941014 0:489498e8dae5 249 if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'H')
dudu941014 0:489498e8dae5 250 {
dudu941014 0:489498e8dae5 251 Velocity_ctrl = 0;
dudu941014 0:489498e8dae5 252 M_Alt = 0;
dudu941014 0:489498e8dae5 253 }
dudu941014 0:489498e8dae5 254
dudu941014 0:489498e8dae5 255 if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'P')
dudu941014 0:489498e8dae5 256 {
dudu941014 0:489498e8dae5 257 pidVelocity.kp = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 258
dudu941014 0:489498e8dae5 259 pidVelocity.kp /= 1000;
dudu941014 0:489498e8dae5 260 }
dudu941014 0:489498e8dae5 261 if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'I')
dudu941014 0:489498e8dae5 262 {
dudu941014 0:489498e8dae5 263 pidVelocity.ki = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 264
dudu941014 0:489498e8dae5 265 pidVelocity.ki /= 1000;
dudu941014 0:489498e8dae5 266 }
dudu941014 0:489498e8dae5 267 if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'D')
dudu941014 0:489498e8dae5 268 {
dudu941014 0:489498e8dae5 269 pidVelocity.kd = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 270
dudu941014 0:489498e8dae5 271 pidVelocity.kd /= 1000;
dudu941014 0:489498e8dae5 272 }
dudu941014 0:489498e8dae5 273
dudu941014 0:489498e8dae5 274 //change the distance
dudu941014 0:489498e8dae5 275 if(usart_rx_buf[0] == 'E' && usart_rx_buf[1] == 'H')
dudu941014 0:489498e8dae5 276 {
dudu941014 0:489498e8dae5 277 pidVelocity.desired = (usart_rx_buf[2] << 8) | usart_rx_buf[3];
dudu941014 0:489498e8dae5 278 if(usart_rx_buf[4]=='-')
dudu941014 0:489498e8dae5 279 pidVelocity.desired = -(pidVelocity.desired /1000);
dudu941014 0:489498e8dae5 280 if(usart_rx_buf[4]=='+')
dudu941014 0:489498e8dae5 281 pidVelocity.desired = pidVelocity.desired /1000;
dudu941014 0:489498e8dae5 282 }
dudu941014 0:489498e8dae5 283
dudu941014 0:489498e8dae5 284 //the following part is to set PID of angle
dudu941014 0:489498e8dae5 285 if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'P')
dudu941014 0:489498e8dae5 286 {
dudu941014 0:489498e8dae5 287 pidAngle.kp = (usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 288
dudu941014 0:489498e8dae5 289 pidAngle.kp /= 1000;
dudu941014 0:489498e8dae5 290 }
dudu941014 0:489498e8dae5 291
dudu941014 0:489498e8dae5 292 if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'I')
dudu941014 0:489498e8dae5 293 {
dudu941014 0:489498e8dae5 294 pidAngle.ki = (usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 295
dudu941014 0:489498e8dae5 296 pidAngle.ki /= 1000;
dudu941014 0:489498e8dae5 297 }
dudu941014 0:489498e8dae5 298
dudu941014 0:489498e8dae5 299 if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'D')
dudu941014 0:489498e8dae5 300 {
dudu941014 0:489498e8dae5 301 pidAngle.kd = ( usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 302
dudu941014 0:489498e8dae5 303 pidAngle.kd /= 1000;
dudu941014 0:489498e8dae5 304 }
dudu941014 0:489498e8dae5 305
dudu941014 0:489498e8dae5 306 if(usart_rx_buf[0] == 'E' && usart_rx_buf[1] == 'P')
dudu941014 0:489498e8dae5 307 {
dudu941014 0:489498e8dae5 308 pidAngle.desired =( usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
dudu941014 0:489498e8dae5 309 if(usart_rx_buf[4]=='-')
dudu941014 0:489498e8dae5 310 pidAngle.desired = -(pidAngle.desired /1000);
dudu941014 0:489498e8dae5 311 if(usart_rx_buf[4]=='+')
dudu941014 0:489498e8dae5 312 pidAngle.desired /= 1000;
dudu941014 0:489498e8dae5 313 }
dudu941014 0:489498e8dae5 314 }
dudu941014 0:489498e8dae5 315 }