YICHUN SAVE US
Dependencies: mbed
Fork of FINALFINALNORMAL by
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?
User | Revision | Line number | New 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 | } |