Robotics ^___^ / Mbed 2 deprecated Bluetooth_try

YICHUN SAVE US

Dependencies:   mbed

Fork of FINALFINALNORMAL by Robotics ^___^

main.cpp@3:6422dc6e8509, 2017-06-08 (annotated)

Committer:
YutingHsiao
Date:
Thu Jun 08 13:44:26 2017 +0000
Revision:
3:6422dc6e8509
Parent:
2:ae0ba466c714
Child:
4:f245d6416dba
final final normal

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
Andy_Lee 1:6228de50cbf4 76 float err_1_old=0;
Andy_Lee 1:6228de50cbf4 77 float err_2_old=0;
Andy_Lee 1:6228de50cbf4 78
Andy_Lee 1:6228de50cbf4 79
Andy_Lee 1:6228de50cbf4 80 Serial pc(USBTX,USBRX);
Andy_Lee 1:6228de50cbf4 81 /////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 82 /////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 83 /////////////////////////////////////////////////////////////
hoting 0:d41917b28387 84 int main()
hoting 0:d41917b28387 85 {
Andy_Lee 1:6228de50cbf4 86 pc.baud(9600);
hoting 0:d41917b28387 87 init_TIMER();
hoting 0:d41917b28387 88 init_UART();
Andy_Lee 1:6228de50cbf4 89 init_PWM();
Andy_Lee 1:6228de50cbf4 90 init_CN();
hoting 0:d41917b28387 91 while(1) {
hoting 0:d41917b28387 92 }
hoting 0:d41917b28387 93 }
hoting 0:d41917b28387 94
hoting 0:d41917b28387 95 void init_TIMER()
hoting 0:d41917b28387 96 {
hoting 0:d41917b28387 97 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 98 }
hoting 0:d41917b28387 99
hoting 0:d41917b28387 100 void init_UART()
hoting 0:d41917b28387 101 {
hoting 0:d41917b28387 102 bt.baud(115200); // baud rate設為115200
hoting 0:d41917b28387 103 bt.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
hoting 0:d41917b28387 104 }
hoting 0:d41917b28387 105
hoting 0:d41917b28387 106 void timer1_ITR()
hoting 0:d41917b28387 107 {
hoting 0:d41917b28387 108 // 避免收到錯誤資料，若超出設定範圍則用上次的資料
hoting 0:d41917b28387 109 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 110 data_received[0] = data_received_old[0];
hoting 0:d41917b28387 111 data_received[1] = data_received_old[1];
hoting 0:d41917b28387 112 data_received[2] = data_received_old[2];
hoting 0:d41917b28387 113 } else {
hoting 0:d41917b28387 114 data_received_old[0] = data_received[0];
hoting 0:d41917b28387 115 data_received_old[1] = data_received[1];
hoting 0:d41917b28387 116 data_received_old[2] = data_received[2];
hoting 0:d41917b28387 117 }
Andy_Lee 1:6228de50cbf4 118
YutingHsiao 3:6422dc6e8509 119 if( data_received_old[2] == 1)
YutingHsiao 3:6422dc6e8509 120 {
YutingHsiao 3:6422dc6e8509 121 //open
YutingHsiao 3:6422dc6e8509 122 // servo_duty = xxx;
Andy_Lee 1:6228de50cbf4 123 }
YutingHsiao 3:6422dc6e8509 124
YutingHsiao 3:6422dc6e8509 125 if( data_received_old[2] == 2)
YutingHsiao 3:6422dc6e8509 126 {
YutingHsiao 3:6422dc6e8509 127 //closed
YutingHsiao 3:6422dc6e8509 128 // servo_duty = xxx;
YutingHsiao 3:6422dc6e8509 129 }
Andy_Lee 1:6228de50cbf4 130
Andy_Lee 1:6228de50cbf4 131 if(servo_duty >= 0.113f)servo_duty = 0.113;
Andy_Lee 1:6228de50cbf4 132 else if(servo_duty <= 0.025f)servo_duty = 0.025;
Andy_Lee 1:6228de50cbf4 133 servo_cmd.write(servo_duty);
YutingHsiao 3:6422dc6e8509 134
YutingHsiao 3:6422dc6e8509 135
YutingHsiao 3:6422dc6e8509 136
Andy_Lee 1:6228de50cbf4 137 // motor1
Andy_Lee 1:6228de50cbf4 138 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
Andy_Lee 1:6228de50cbf4 139 speed_count_1 = 0;
Andy_Lee 1:6228de50cbf4 140
Andy_Lee 1:6228de50cbf4 141 ///PI controller for motor1///
Andy_Lee 1:6228de50cbf4 142
Andy_Lee 1:6228de50cbf4 143 err_1=(data_received_old[0]-rotation_speed_1)*p_gain;
Andy_Lee 1:6228de50cbf4 144 ierr_1=(err_1_old+err_1)*i_gain;
Andy_Lee 1:6228de50cbf4 145 PI_out_1=err_1+ierr_1;
Andy_Lee 1:6228de50cbf4 146 err_1_old=err_1;
Andy_Lee 1:6228de50cbf4 147 //////////////////////////////
Andy_Lee 1:6228de50cbf4 148
Andy_Lee 1:6228de50cbf4 149 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
Andy_Lee 1:6228de50cbf4 150 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
Andy_Lee 1:6228de50cbf4 151 pwm1_duty = PI_out_1 + 0.5f;
Andy_Lee 1:6228de50cbf4 152 pwm1.write(pwm1_duty);
Andy_Lee 1:6228de50cbf4 153 TIM1->CCER |= 0x4;
Andy_Lee 1:6228de50cbf4 154
Andy_Lee 1:6228de50cbf4 155 //motor2
Andy_Lee 1:6228de50cbf4 156 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
Andy_Lee 1:6228de50cbf4 157 speed_count_2 = 0;
Andy_Lee 1:6228de50cbf4 158
Andy_Lee 1:6228de50cbf4 159 ///PI controller for motor2///
Andy_Lee 1:6228de50cbf4 160
Andy_Lee 1:6228de50cbf4 161 err_2=(data_received_old[1]-rotation_speed_2)*p_gain;
Andy_Lee 1:6228de50cbf4 162 ierr_2=(err_2_old+err_2)*i_gain;
Andy_Lee 1:6228de50cbf4 163 PI_out_2=err_2+ierr_2;
Andy_Lee 1:6228de50cbf4 164 err_2_old=err_2;
Andy_Lee 1:6228de50cbf4 165 //////////////////////////////
Andy_Lee 1:6228de50cbf4 166
Andy_Lee 1:6228de50cbf4 167 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
Andy_Lee 1:6228de50cbf4 168 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
YutingHsiao 2:ae0ba466c714 169 pwm2_duty = -PI_out_2 + 0.5f;
Andy_Lee 1:6228de50cbf4 170 pwm2.write(pwm2_duty);
Andy_Lee 1:6228de50cbf4 171 TIM1->CCER |= 0x40;
Andy_Lee 1:6228de50cbf4 172
Andy_Lee 1:6228de50cbf4 173 //pc.printf("SPEED= %f,/n",data_received_old[1]);
Andy_Lee 1:6228de50cbf4 174
hoting 0:d41917b28387 175 }
hoting 0:d41917b28387 176
hoting 0:d41917b28387 177 void RX_ITR()
hoting 0:d41917b28387 178 {
hoting 0:d41917b28387 179 while(bt.readable()) {
hoting 0:d41917b28387 180 static char uart_read;
hoting 0:d41917b28387 181 uart_read = bt.getc();
hoting 0:d41917b28387 182 if(uart_read == 127 && RX_flag1 == 1) {
hoting 0:d41917b28387 183 RX_flag2 = 1;
hoting 0:d41917b28387 184 } else {
hoting 0:d41917b28387 185 RX_flag1 = 0;
hoting 0:d41917b28387 186 }
hoting 0:d41917b28387 187
hoting 0:d41917b28387 188 if(RX_flag2 == 1) {
hoting 0:d41917b28387 189 getData[readcount] = uart_read;
hoting 0:d41917b28387 190 readcount++;
Andy_Lee 1:6228de50cbf4 191 if(readcount >= 5) {
hoting 0:d41917b28387 192 readcount = 0;
hoting 0:d41917b28387 193 RX_flag2 = 0;
hoting 0:d41917b28387 194 ///code for decoding///
Andy_Lee 1:6228de50cbf4 195 data_received[0] = (getData[2] << 8) | getData[1];
Andy_Lee 1:6228de50cbf4 196 data_received[1] = (getData[4] << 8) | getData[3];
Andy_Lee 1:6228de50cbf4 197 data_received[2] = getData[5];
hoting 0:d41917b28387 198 ///////////////////////
hoting 0:d41917b28387 199 }
Andy_Lee 1:6228de50cbf4 200 }
Andy_Lee 1:6228de50cbf4 201 else if(uart_read == 254 && RX_flag1 == 0) {
hoting 0:d41917b28387 202 RX_flag1 = 1;
hoting 0:d41917b28387 203 }
hoting 0:d41917b28387 204 }
Andy_Lee 1:6228de50cbf4 205 }
Andy_Lee 1:6228de50cbf4 206
Andy_Lee 1:6228de50cbf4 207 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 208 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 209 ///////////////////////////////////////////////////////////////////
Andy_Lee 1:6228de50cbf4 210 void init_PWM()
Andy_Lee 1:6228de50cbf4 211 {
Andy_Lee 1:6228de50cbf4 212 servo_cmd.period_ms(20);
Andy_Lee 1:6228de50cbf4 213 servo_cmd.write(servo_duty);
Andy_Lee 1:6228de50cbf4 214
Andy_Lee 1:6228de50cbf4 215 pwm1.period_us(50);
Andy_Lee 1:6228de50cbf4 216 pwm1.write(0.5);
Andy_Lee 1:6228de50cbf4 217 TIM1->CCER |= 0x4;
Andy_Lee 1:6228de50cbf4 218
Andy_Lee 1:6228de50cbf4 219 pwm2.period_us(50);
Andy_Lee 1:6228de50cbf4 220 pwm2.write(0.5);
Andy_Lee 1:6228de50cbf4 221 TIM1->CCER |= 0x40;
Andy_Lee 1:6228de50cbf4 222 }
Andy_Lee 1:6228de50cbf4 223 void init_CN()
Andy_Lee 1:6228de50cbf4 224 {
Andy_Lee 1:6228de50cbf4 225 HallA.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 226 HallA.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 227 HallB.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 228 HallB.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 229
Andy_Lee 1:6228de50cbf4 230 HallA_2.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 231 HallA_2.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 232 HallB_2.rise(&CN_ITR);
Andy_Lee 1:6228de50cbf4 233 HallB_2.fall(&CN_ITR);
Andy_Lee 1:6228de50cbf4 234 }
Andy_Lee 1:6228de50cbf4 235
Andy_Lee 1:6228de50cbf4 236 void CN_ITR()
Andy_Lee 1:6228de50cbf4 237 {
YutingHsiao 2:ae0ba466c714 238
Andy_Lee 1:6228de50cbf4 239 // motor1
Andy_Lee 1:6228de50cbf4 240 HallA_1_state = HallA.read();
Andy_Lee 1:6228de50cbf4 241 HallB_1_state = HallB.read();
YutingHsiao 2:ae0ba466c714 242 //led1 != led1;
Andy_Lee 1:6228de50cbf4 243
Andy_Lee 1:6228de50cbf4 244 ///code for state determination///
YutingHsiao 2:ae0ba466c714 245 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 246 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 247 //determine the state
YutingHsiao 2:ae0ba466c714 248 if((HallA_1_state == 0)&&(HallB_1_state == 0))
YutingHsiao 2:ae0ba466c714 249 {
YutingHsiao 2:ae0ba466c714 250 state_1 = 1;
YutingHsiao 2:ae0ba466c714 251 }
YutingHsiao 2:ae0ba466c714 252 else if((HallA_1_state == 0)&&(HallB_1_state == 1))
YutingHsiao 2:ae0ba466c714 253 {
YutingHsiao 2:ae0ba466c714 254 state_1 = 2;
YutingHsiao 2:ae0ba466c714 255 }
YutingHsiao 2:ae0ba466c714 256 else if((HallA_1_state == 1)&&(HallB_1_state == 1))
YutingHsiao 2:ae0ba466c714 257 {
YutingHsiao 2:ae0ba466c714 258 state_1 = 3;
YutingHsiao 2:ae0ba466c714 259 }
YutingHsiao 2:ae0ba466c714 260 else if((HallA_1_state == 1)&&(HallB_1_state ==0))
YutingHsiao 2:ae0ba466c714 261 {
YutingHsiao 2:ae0ba466c714 262 state_1 = 4;
YutingHsiao 2:ae0ba466c714 263 }
YutingHsiao 2:ae0ba466c714 264
YutingHsiao 2:ae0ba466c714 265 //forward or backward
YutingHsiao 2:ae0ba466c714 266 int direction_1 = 0;
YutingHsiao 2:ae0ba466c714 267 direction_1 = state_1 - state_1_old;
YutingHsiao 2:ae0ba466c714 268 if((direction_1 == -1) || (direction_1 == 3))
YutingHsiao 2:ae0ba466c714 269 {
YutingHsiao 2:ae0ba466c714 270 //forward
YutingHsiao 2:ae0ba466c714 271 speed_count_1 = speed_count_1 - 1;
YutingHsiao 2:ae0ba466c714 272 }
YutingHsiao 2:ae0ba466c714 273 else if((direction_1 == 1) || (direction_1 == -3))
YutingHsiao 2:ae0ba466c714 274 {
YutingHsiao 2:ae0ba466c714 275 //backward
YutingHsiao 2:ae0ba466c714 276 speed_count_1 = speed_count_1 + 1;
YutingHsiao 2:ae0ba466c714 277 }
YutingHsiao 2:ae0ba466c714 278 else
YutingHsiao 2:ae0ba466c714 279 {
YutingHsiao 2:ae0ba466c714 280 //prevent initializing error
YutingHsiao 2:ae0ba466c714 281 state_1_old = state_1;
YutingHsiao 2:ae0ba466c714 282 }
YutingHsiao 2:ae0ba466c714 283
YutingHsiao 2:ae0ba466c714 284 //change old state
YutingHsiao 2:ae0ba466c714 285 state_1_old = state_1;
YutingHsiao 2:ae0ba466c714 286 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 287 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 288 //forward : speed_count_1 + 1
Andy_Lee 1:6228de50cbf4 289 //backward : speed_count_1 - 1
Andy_Lee 1:6228de50cbf4 290
Andy_Lee 1:6228de50cbf4 291 // motor2
Andy_Lee 1:6228de50cbf4 292 HallA_2_state = HallA_2.read();
Andy_Lee 1:6228de50cbf4 293 HallB_2_state = HallB_2.read();
Andy_Lee 1:6228de50cbf4 294
Andy_Lee 1:6228de50cbf4 295 ///code for state determination///
Andy_Lee 1:6228de50cbf4 296 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 297 /////////////////////////////////
YutingHsiao 2:ae0ba466c714 298 //determine the state
YutingHsiao 2:ae0ba466c714 299 if((HallA_2_state == 0)&&(HallB_2_state == 0))
YutingHsiao 2:ae0ba466c714 300 {
YutingHsiao 2:ae0ba466c714 301 state_2 = 1;
YutingHsiao 2:ae0ba466c714 302 }
YutingHsiao 2:ae0ba466c714 303 else if((HallA_2_state == 0)&&(HallB_2_state == 1))
YutingHsiao 2:ae0ba466c714 304 {
YutingHsiao 2:ae0ba466c714 305 state_2 = 2;
YutingHsiao 2:ae0ba466c714 306 }
YutingHsiao 2:ae0ba466c714 307 else if((HallA_2_state == 1)&&(HallB_2_state == 1))
YutingHsiao 2:ae0ba466c714 308 {
YutingHsiao 2:ae0ba466c714 309 state_2 = 3;
YutingHsiao 2:ae0ba466c714 310 }
YutingHsiao 2:ae0ba466c714 311 else if((HallA_2_state == 1)&&(HallB_2_state ==0))
YutingHsiao 2:ae0ba466c714 312 {
YutingHsiao 2:ae0ba466c714 313 state_2 = 4;
YutingHsiao 2:ae0ba466c714 314 }
YutingHsiao 2:ae0ba466c714 315
YutingHsiao 2:ae0ba466c714 316 //forward or backward
YutingHsiao 2:ae0ba466c714 317 int direction_2 = 0;
YutingHsiao 2:ae0ba466c714 318 direction_2 = state_2 - state_2_old;
YutingHsiao 2:ae0ba466c714 319 if((direction_2 == 1) || (direction_2 == -3))
YutingHsiao 2:ae0ba466c714 320 {
YutingHsiao 2:ae0ba466c714 321 //forward
YutingHsiao 2:ae0ba466c714 322 speed_count_2 = speed_count_2 - 1;
YutingHsiao 2:ae0ba466c714 323 }
YutingHsiao 2:ae0ba466c714 324 else if((direction_2 == -1) || (direction_2 == 3))
YutingHsiao 2:ae0ba466c714 325 {
YutingHsiao 2:ae0ba466c714 326 //backward
YutingHsiao 2:ae0ba466c714 327 speed_count_2 = speed_count_2 + 1;
YutingHsiao 2:ae0ba466c714 328 }
YutingHsiao 2:ae0ba466c714 329 else
YutingHsiao 2:ae0ba466c714 330 {
YutingHsiao 2:ae0ba466c714 331 //prevent initializing error
YutingHsiao 2:ae0ba466c714 332 state_2_old = state_2;
YutingHsiao 2:ae0ba466c714 333 }
YutingHsiao 2:ae0ba466c714 334
YutingHsiao 2:ae0ba466c714 335 //change old state
YutingHsiao 2:ae0ba466c714 336 state_2_old = state_2;
YutingHsiao 2:ae0ba466c714 337 //////////////////////////////////
YutingHsiao 2:ae0ba466c714 338 //////////////////////////////////
Andy_Lee 1:6228de50cbf4 339 //forward : speed_count_2 + 1
Andy_Lee 1:6228de50cbf4 340 //backward : speed_count_2 - 1
YutingHsiao 2:ae0ba466c714 341 }