Robotics ^___^ / Mbed 2 deprecated FINAL

final

Dependencies:   mbed

Fork of Robotics_LAB_UART by NTHUPME_Robotics_2017

main.cpp@2:ae0ba466c714, 2017-04-25 (annotated)

Committer:
YutingHsiao
Date:
Tue Apr 25 09:20:23 2017 +0000
Revision:
2:ae0ba466c714
Parent:
1:6228de50cbf4 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:d41917b28387 1 #include "mbed.h"
hoting 0:d41917b28387 2
hoting 0:d41917b28387 3 Ticker timer1;
hoting 0:d41917b28387 4 Serial bt(D10, D2); // TXpin, RXpin
hoting 0:d41917b28387 5
hoting 0:d41917b28387 6 //RX
hoting 0:d41917b28387 7 int readcount = 0;
hoting 0:d41917b28387 8 int RX_flag1 = 0;
hoting 0:d41917b28387 9 int RX_flag2 = 0;
hoting 0:d41917b28387 10 char getData[6] = {0,0,0,0,0,0};
hoting 0:d41917b28387 11 short data_received[3] = {0,0,0};
hoting 0:d41917b28387 12 short data_received_old[3] = {0,0,0};
hoting 0:d41917b28387 13
hoting 0:d41917b28387 14 //函式宣告
hoting 0:d41917b28387 15 void init_TIMER();
hoting 0:d41917b28387 16 void timer1_ITR();
hoting 0:d41917b28387 17 void init_UART();
hoting 0:d41917b28387 18 void RX_ITR();
hoting 0:d41917b28387 19
Andy_Lee 1:6228de50cbf4 20 /////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 21 /////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 22 /////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 23 // servo motor
Andy_Lee 1:6228de50cbf4 24 PwmOut servo_cmd(A0);
Andy_Lee 1:6228de50cbf4 25 // DC motor
Andy_Lee 1:6228de50cbf4 26 PwmOut pwm1(D7);
Andy_Lee 1:6228de50cbf4 27 PwmOut pwm1n(D11);
Andy_Lee 1:6228de50cbf4 28 PwmOut pwm2(D8);
Andy_Lee 1:6228de50cbf4 29 PwmOut pwm2n(A3);
Andy_Lee 1:6228de50cbf4 30
Andy_Lee 1:6228de50cbf4 31 // Motor1 sensor
Andy_Lee 1:6228de50cbf4 32 InterruptIn HallA(A1);
Andy_Lee 1:6228de50cbf4 33 InterruptIn HallB(A2);
Andy_Lee 1:6228de50cbf4 34 // Motor2 sensor
Andy_Lee 1:6228de50cbf4 35 InterruptIn HallA_2(D13);
Andy_Lee 1:6228de50cbf4 36 InterruptIn HallB_2(D12);
Andy_Lee 1:6228de50cbf4 37
Andy_Lee 1:6228de50cbf4 38 void init_CN();
Andy_Lee 1:6228de50cbf4 39 void CN_ITR();
Andy_Lee 1:6228de50cbf4 40 void init_PWM();
Andy_Lee 1:6228de50cbf4 41
Andy_Lee 1:6228de50cbf4 42 // servo motor
Andy_Lee 1:6228de50cbf4 43 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
Andy_Lee 1:6228de50cbf4 44 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
Andy_Lee 1:6228de50cbf4 45 int angle = 0;
Andy_Lee 1:6228de50cbf4 46
Andy_Lee 1:6228de50cbf4 47 // Hall sensor
Andy_Lee 1:6228de50cbf4 48 int HallA_1_state = 0;
Andy_Lee 1:6228de50cbf4 49 int HallB_1_state = 0;
Andy_Lee 1:6228de50cbf4 50 int state_1 = 0;
Andy_Lee 1:6228de50cbf4 51 int state_1_old = 0;
Andy_Lee 1:6228de50cbf4 52 int HallA_2_state = 0;
Andy_Lee 1:6228de50cbf4 53 int HallB_2_state = 0;
Andy_Lee 1:6228de50cbf4 54 int state_2 = 0;
Andy_Lee 1:6228de50cbf4 55 int state_2_old = 0;
Andy_Lee 1:6228de50cbf4 56 int i=0;
Andy_Lee 1:6228de50cbf4 57
Andy_Lee 1:6228de50cbf4 58 // DC motor rotation speed control
Andy_Lee 1:6228de50cbf4 59 int speed_count_1 = 0;
Andy_Lee 1:6228de50cbf4 60 float rotation_speed_1 = 0.0;
Andy_Lee 1:6228de50cbf4 61 float rotation_speed_ref_1 = 0;
Andy_Lee 1:6228de50cbf4 62 float pwm1_duty = 0.5;
Andy_Lee 1:6228de50cbf4 63 float PI_out_1 = 0.0;
Andy_Lee 1:6228de50cbf4 64 float err_1 = 0.0;
Andy_Lee 1:6228de50cbf4 65 float ierr_1 = 0.0;
Andy_Lee 1:6228de50cbf4 66 int speed_count_2 = 0;
Andy_Lee 1:6228de50cbf4 67 float rotation_speed_2 = 0.0;
Andy_Lee 1:6228de50cbf4 68 float rotation_speed_ref_2 = 0;
Andy_Lee 1:6228de50cbf4 69 float pwm2_duty = 0.5;
Andy_Lee 1:6228de50cbf4 70 float PI_out_2 = 0.0;
Andy_Lee 1:6228de50cbf4 71 float err_2 = 0.0;
Andy_Lee 1:6228de50cbf4 72 float ierr_2 = 0.0;
YutingHsiao 2:ae0ba466c714 73 float p_gain=0.002;
YutingHsiao 2:ae0ba466c714 74 float i_gain=0.05;
Andy_Lee 1:6228de50cbf4 75 float duty;
Andy_Lee 1:6228de50cbf4 76
Andy_Lee 1:6228de50cbf4 77 float err_1_old=0;
Andy_Lee 1:6228de50cbf4 78 float err_2_old=0;
Andy_Lee 1:6228de50cbf4 79 bool servo=1;
Andy_Lee 1:6228de50cbf4 80
Andy_Lee 1:6228de50cbf4 81
Andy_Lee 1:6228de50cbf4 82 Serial pc(USBTX,USBRX);
Andy_Lee 1:6228de50cbf4 83 /////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 84 /////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 85 /////////////////////////////////////////////////////////////
hoting 0:d41917b28387 86 int main()
hoting 0:d41917b28387 87 {
Andy_Lee 1:6228de50cbf4 88 pc.baud(9600);
hoting 0:d41917b28387 89 init_TIMER();
hoting 0:d41917b28387 90 init_UART();
Andy_Lee 1:6228de50cbf4 91 init_PWM();
Andy_Lee 1:6228de50cbf4 92 init_CN();
hoting 0:d41917b28387 93 while(1) {
hoting 0:d41917b28387 94 }
hoting 0:d41917b28387 95 }
hoting 0:d41917b28387 96
hoting 0:d41917b28387 97 void init_TIMER()
hoting 0:d41917b28387 98 {
hoting 0:d41917b28387 99 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
hoting 0:d41917b28387 100 }
hoting 0:d41917b28387 101
hoting 0:d41917b28387 102 void init_UART()
hoting 0:d41917b28387 103 {
hoting 0:d41917b28387 104 bt.baud(115200); // baud rate設為115200
hoting 0:d41917b28387 105 bt.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
hoting 0:d41917b28387 106 }
hoting 0:d41917b28387 107
hoting 0:d41917b28387 108 void timer1_ITR()
hoting 0:d41917b28387 109 {
hoting 0:d41917b28387 110 // 避免收到錯誤資料，若超出設定範圍則用上次的資料
hoting 0:d41917b28387 111 if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) {
hoting 0:d41917b28387 112 data_received[0] = data_received_old[0];
hoting 0:d41917b28387 113 data_received[1] = data_received_old[1];
hoting 0:d41917b28387 114 data_received[2] = data_received_old[2];
hoting 0:d41917b28387 115 } else {
hoting 0:d41917b28387 116 data_received_old[0] = data_received[0];
hoting 0:d41917b28387 117 data_received_old[1] = data_received[1];
hoting 0:d41917b28387 118 data_received_old[2] = data_received[2];
hoting 0:d41917b28387 119 }
Andy_Lee 1:6228de50cbf4 120
Andy_Lee 1:6228de50cbf4 121 /*
Andy_Lee 1:6228de50cbf4 122 servo=1;
Andy_Lee 1:6228de50cbf4 123 while(servo==1){
Andy_Lee 1:6228de50cbf4 124 if(i==0 || i==100 || i==200 || i==300 || i==400 || i==500 || i==600){
Andy_Lee 1:6228de50cbf4 125
Andy_Lee 1:6228de50cbf4 126 duty=0.113f-0.0000733f*(float)i;
Andy_Lee 1:6228de50cbf4 127
Andy_Lee 1:6228de50cbf4 128 }
Andy_Lee 1:6228de50cbf4 129 //pc.printf("duty= %f ,\n",duty);
Andy_Lee 1:6228de50cbf4 130 //servo_cmd.period_ms(20);
Andy_Lee 1:6228de50cbf4 131 servo_cmd.write( data_received_old[2]);
Andy_Lee 1:6228de50cbf4 132 servo=0;
Andy_Lee 1:6228de50cbf4 133 i=i+1;
Andy_Lee 1:6228de50cbf4 134 }
Andy_Lee 1:6228de50cbf4 135 if(i==601){
Andy_Lee 1:6228de50cbf4 136 i=0;
Andy_Lee 1:6228de50cbf4 137 }
Andy_Lee 1:6228de50cbf4 138
Andy_Lee 1:6228de50cbf4 139
Andy_Lee 1:6228de50cbf4 140 */
Andy_Lee 1:6228de50cbf4 141 /////////////////////////
Andy_Lee 1:6228de50cbf4 142
Andy_Lee 1:6228de50cbf4 143 if(servo_duty >= 0.113f)servo_duty = 0.113;
Andy_Lee 1:6228de50cbf4 144 else if(servo_duty <= 0.025f)servo_duty = 0.025;
Andy_Lee 1:6228de50cbf4 145 servo_cmd.write(servo_duty);
Andy_Lee 1:6228de50cbf4 146
Andy_Lee 1:6228de50cbf4 147 // motor1
Andy_Lee 1:6228de50cbf4 148 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
Andy_Lee 1:6228de50cbf4 149 speed_count_1 = 0;
Andy_Lee 1:6228de50cbf4 150
Andy_Lee 1:6228de50cbf4 151 ///PI controller for motor1///
Andy_Lee 1:6228de50cbf4 152
Andy_Lee 1:6228de50cbf4 153 err_1=(data_received_old[0]-rotation_speed_1)*p_gain;
Andy_Lee 1:6228de50cbf4 154 ierr_1=(err_1_old+err_1)*i_gain;
Andy_Lee 1:6228de50cbf4 155 PI_out_1=err_1+ierr_1;
Andy_Lee 1:6228de50cbf4 156 err_1_old=err_1;
Andy_Lee 1:6228de50cbf4 157 //////////////////////////////
Andy_Lee 1:6228de50cbf4 158
Andy_Lee 1:6228de50cbf4 159 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
Andy_Lee 1:6228de50cbf4 160 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
Andy_Lee 1:6228de50cbf4 161 pwm1_duty = PI_out_1 + 0.5f;
Andy_Lee 1:6228de50cbf4 162 pwm1.write(pwm1_duty);
Andy_Lee 1:6228de50cbf4 163 TIM1->CCER |= 0x4;
Andy_Lee 1:6228de50cbf4 164
Andy_Lee 1:6228de50cbf4 165 //motor2
Andy_Lee 1:6228de50cbf4 166 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
Andy_Lee 1:6228de50cbf4 167 speed_count_2 = 0;
Andy_Lee 1:6228de50cbf4 168
Andy_Lee 1:6228de50cbf4 169 ///PI controller for motor2///
Andy_Lee 1:6228de50cbf4 170
Andy_Lee 1:6228de50cbf4 171 err_2=(data_received_old[1]-rotation_speed_2)*p_gain;
Andy_Lee 1:6228de50cbf4 172 ierr_2=(err_2_old+err_2)*i_gain;
Andy_Lee 1:6228de50cbf4 173 PI_out_2=err_2+ierr_2;
Andy_Lee 1:6228de50cbf4 174 err_2_old=err_2;
Andy_Lee 1:6228de50cbf4 175 //////////////////////////////
Andy_Lee 1:6228de50cbf4 176
Andy_Lee 1:6228de50cbf4 177 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
Andy_Lee 1:6228de50cbf4 178 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
YutingHsiao 2:ae0ba466c714 179 pwm2_duty = -PI_out_2 + 0.5f;
Andy_Lee 1:6228de50cbf4 180 pwm2.write(pwm2_duty);
Andy_Lee 1:6228de50cbf4 181 TIM1->CCER |= 0x40;
Andy_Lee 1:6228de50cbf4 182
Andy_Lee 1:6228de50cbf4 183 //pc.printf("SPEED= %f,/n",data_received_old[1]);
Andy_Lee 1:6228de50cbf4 184
hoting 0:d41917b28387 185 }
hoting 0:d41917b28387 186
hoting 0:d41917b28387 187 void RX_ITR()
hoting 0:d41917b28387 188 {
hoting 0:d41917b28387 189 while(bt.readable()) {
hoting 0:d41917b28387 190 static char uart_read;
hoting 0:d41917b28387 191 uart_read = bt.getc();
hoting 0:d41917b28387 192 if(uart_read == 127 && RX_flag1 == 1) {
hoting 0:d41917b28387 193 RX_flag2 = 1;
hoting 0:d41917b28387 194 } else {
hoting 0:d41917b28387 195 RX_flag1 = 0;
hoting 0:d41917b28387 196 }
hoting 0:d41917b28387 197
hoting 0:d41917b28387 198 if(RX_flag2 == 1) {
hoting 0:d41917b28387 199 getData[readcount] = uart_read;
hoting 0:d41917b28387 200 readcount++;
Andy_Lee 1:6228de50cbf4 201 if(readcount >= 5) {
hoting 0:d41917b28387 202 readcount = 0;
hoting 0:d41917b28387 203 RX_flag2 = 0;
hoting 0:d41917b28387 204 ///code for decoding///
Andy_Lee 1:6228de50cbf4 205 data_received[0] = (getData[2] << 8) | getData[1];
Andy_Lee 1:6228de50cbf4 206 data_received[1] = (getData[4] << 8) | getData[3];
Andy_Lee 1:6228de50cbf4 207 data_received[2] = getData[5];
hoting 0:d41917b28387 208 ///////////////////////
hoting 0:d41917b28387 209 }
Andy_Lee 1:6228de50cbf4 210 }
Andy_Lee 1:6228de50cbf4 211 else if(uart_read == 254 && RX_flag1 == 0) {
hoting 0:d41917b28387 212 RX_flag1 = 1;
hoting 0:d41917b28387 213 }
hoting 0:d41917b28387 214 }
Andy_Lee 1:6228de50cbf4 215 }
Andy_Lee 1:6228de50cbf4 216
Andy_Lee 1:6228de50cbf4 217 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 218 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 219 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 220 void init_PWM()
Andy_Lee 1:6228de50cbf4 221 {
Andy_Lee 1:6228de50cbf4 222 servo_cmd.period_ms(20);
Andy_Lee 1:6228de50cbf4 223 servo_cmd.write(servo_duty);
Andy_Lee 1:6228de50cbf4 224
Andy_Lee 1:6228de50cbf4 225 pwm1.period_us(50);
Andy_Lee 1:6228de50cbf4 226 pwm1.write(0.5);
Andy_Lee 1:6228de50cbf4 227 TIM1->CCER |= 0x4;
Andy_Lee 1:6228de50cbf4 228
Andy_Lee 1:6228de50cbf4 229 pwm2.period_us(50);
Andy_Lee 1:6228de50cbf4 230 pwm2.write(0.5);
Andy_Lee 1:6228de50cbf4 231 TIM1->CCER |= 0x40;
Andy_Lee 1:6228de50cbf4 232 }
Andy_Lee 1:6228de50cbf4 233 void init_CN()
Andy_Lee 1:6228de50cbf4 234 {
Andy_Lee 1:6228de50cbf4 235 HallA.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 236 HallA.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 237 HallB.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 238 HallB.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 239
Andy_Lee 1:6228de50cbf4 240 HallA_2.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 241 HallA_2.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 242 HallB_2.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 243 HallB_2.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 244 }
Andy_Lee 1:6228de50cbf4 245
Andy_Lee 1:6228de50cbf4 246 void CN_ITR()
Andy_Lee 1:6228de50cbf4 247 {
YutingHsiao 2:ae0ba466c714 248
Andy_Lee 1:6228de50cbf4 249 // motor1
Andy_Lee 1:6228de50cbf4 250 HallA_1_state = HallA.read();
Andy_Lee 1:6228de50cbf4 251 HallB_1_state = HallB.read();
YutingHsiao 2:ae0ba466c714 252 //led1 != led1;
Andy_Lee 1:6228de50cbf4 253
Andy_Lee 1:6228de50cbf4 254 ///code for state determination///
YutingHsiao 2:ae0ba466c714 255 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 256 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 257 //determine the state
YutingHsiao 2:ae0ba466c714 258 if((HallA_1_state == 0)&&(HallB_1_state == 0))
YutingHsiao 2:ae0ba466c714 259 {
YutingHsiao 2:ae0ba466c714 260 state_1 = 1;
YutingHsiao 2:ae0ba466c714 261 }
YutingHsiao 2:ae0ba466c714 262 else if((HallA_1_state == 0)&&(HallB_1_state == 1))
YutingHsiao 2:ae0ba466c714 263 {
YutingHsiao 2:ae0ba466c714 264 state_1 = 2;
YutingHsiao 2:ae0ba466c714 265 }
YutingHsiao 2:ae0ba466c714 266 else if((HallA_1_state == 1)&&(HallB_1_state == 1))
YutingHsiao 2:ae0ba466c714 267 {
YutingHsiao 2:ae0ba466c714 268 state_1 = 3;
YutingHsiao 2:ae0ba466c714 269 }
YutingHsiao 2:ae0ba466c714 270 else if((HallA_1_state == 1)&&(HallB_1_state ==0))
YutingHsiao 2:ae0ba466c714 271 {
YutingHsiao 2:ae0ba466c714 272 state_1 = 4;
YutingHsiao 2:ae0ba466c714 273 }
YutingHsiao 2:ae0ba466c714 274
YutingHsiao 2:ae0ba466c714 275 //forward or backward
YutingHsiao 2:ae0ba466c714 276 int direction_1 = 0;
YutingHsiao 2:ae0ba466c714 277 direction_1 = state_1 - state_1_old;
YutingHsiao 2:ae0ba466c714 278 if((direction_1 == -1) || (direction_1 == 3))
YutingHsiao 2:ae0ba466c714 279 {
YutingHsiao 2:ae0ba466c714 280 //forward
YutingHsiao 2:ae0ba466c714 281 speed_count_1 = speed_count_1 - 1;
YutingHsiao 2:ae0ba466c714 282 }
YutingHsiao 2:ae0ba466c714 283 else if((direction_1 == 1) || (direction_1 == -3))
YutingHsiao 2:ae0ba466c714 284 {
YutingHsiao 2:ae0ba466c714 285 //backward
YutingHsiao 2:ae0ba466c714 286 speed_count_1 = speed_count_1 + 1;
YutingHsiao 2:ae0ba466c714 287 }
YutingHsiao 2:ae0ba466c714 288 else
YutingHsiao 2:ae0ba466c714 289 {
YutingHsiao 2:ae0ba466c714 290 //prevent initializing error
YutingHsiao 2:ae0ba466c714 291 state_1_old = state_1;
YutingHsiao 2:ae0ba466c714 292 }
YutingHsiao 2:ae0ba466c714 293
YutingHsiao 2:ae0ba466c714 294 //change old state
YutingHsiao 2:ae0ba466c714 295 state_1_old = state_1;
YutingHsiao 2:ae0ba466c714 296 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 297 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 298 //forward : speed_count_1 + 1
Andy_Lee 1:6228de50cbf4 299 //backward : speed_count_1 - 1
Andy_Lee 1:6228de50cbf4 300
Andy_Lee 1:6228de50cbf4 301 // motor2
Andy_Lee 1:6228de50cbf4 302 HallA_2_state = HallA_2.read();
Andy_Lee 1:6228de50cbf4 303 HallB_2_state = HallB_2.read();
Andy_Lee 1:6228de50cbf4 304
Andy_Lee 1:6228de50cbf4 305 ///code for state determination///
Andy_Lee 1:6228de50cbf4 306 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 307 /////////////////////////////////
YutingHsiao 2:ae0ba466c714 308 //determine the state
YutingHsiao 2:ae0ba466c714 309 if((HallA_2_state == 0)&&(HallB_2_state == 0))
YutingHsiao 2:ae0ba466c714 310 {
YutingHsiao 2:ae0ba466c714 311 state_2 = 1;
YutingHsiao 2:ae0ba466c714 312 }
YutingHsiao 2:ae0ba466c714 313 else if((HallA_2_state == 0)&&(HallB_2_state == 1))
YutingHsiao 2:ae0ba466c714 314 {
YutingHsiao 2:ae0ba466c714 315 state_2 = 2;
YutingHsiao 2:ae0ba466c714 316 }
YutingHsiao 2:ae0ba466c714 317 else if((HallA_2_state == 1)&&(HallB_2_state == 1))
YutingHsiao 2:ae0ba466c714 318 {
YutingHsiao 2:ae0ba466c714 319 state_2 = 3;
YutingHsiao 2:ae0ba466c714 320 }
YutingHsiao 2:ae0ba466c714 321 else if((HallA_2_state == 1)&&(HallB_2_state ==0))
YutingHsiao 2:ae0ba466c714 322 {
YutingHsiao 2:ae0ba466c714 323 state_2 = 4;
YutingHsiao 2:ae0ba466c714 324 }
YutingHsiao 2:ae0ba466c714 325
YutingHsiao 2:ae0ba466c714 326 //forward or backward
YutingHsiao 2:ae0ba466c714 327 int direction_2 = 0;
YutingHsiao 2:ae0ba466c714 328 direction_2 = state_2 - state_2_old;
YutingHsiao 2:ae0ba466c714 329 if((direction_2 == 1) || (direction_2 == -3))
YutingHsiao 2:ae0ba466c714 330 {
YutingHsiao 2:ae0ba466c714 331 //forward
YutingHsiao 2:ae0ba466c714 332 speed_count_2 = speed_count_2 - 1;
YutingHsiao 2:ae0ba466c714 333 }
YutingHsiao 2:ae0ba466c714 334 else if((direction_2 == -1) || (direction_2 == 3))
YutingHsiao 2:ae0ba466c714 335 {
YutingHsiao 2:ae0ba466c714 336 //backward
YutingHsiao 2:ae0ba466c714 337 speed_count_2 = speed_count_2 + 1;
YutingHsiao 2:ae0ba466c714 338 }
YutingHsiao 2:ae0ba466c714 339 else
YutingHsiao 2:ae0ba466c714 340 {
YutingHsiao 2:ae0ba466c714 341 //prevent initializing error
YutingHsiao 2:ae0ba466c714 342 state_2_old = state_2;
YutingHsiao 2:ae0ba466c714 343 }
YutingHsiao 2:ae0ba466c714 344
YutingHsiao 2:ae0ba466c714 345 //change old state
YutingHsiao 2:ae0ba466c714 346 state_2_old = state_2;
YutingHsiao 2:ae0ba466c714 347 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 348 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 349 //forward : speed_count_2 + 1
Andy_Lee 1:6228de50cbf4 350 //backward : speed_count_2 - 1
YutingHsiao 2:ae0ba466c714 351 }