YICHUN SAVE US
Dependencies: mbed
Fork of FINALFINALNORMAL by
main.cpp@1:6228de50cbf4, 2017-04-06 (annotated)
- Committer:
- Andy_Lee
- Date:
- Thu Apr 06 11:07:28 2017 +0000
- Revision:
- 1:6228de50cbf4
- Parent:
- 0:d41917b28387
- Child:
- 2:ae0ba466c714
done
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; |
Andy_Lee | 1:6228de50cbf4 | 73 | float p_gain=0.015; |
Andy_Lee | 1:6228de50cbf4 | 74 | float i_gain=0.003; |
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; |
Andy_Lee | 1:6228de50cbf4 | 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 | { |
Andy_Lee | 1:6228de50cbf4 | 248 | // motor1 |
Andy_Lee | 1:6228de50cbf4 | 249 | HallA_1_state = HallA.read(); |
Andy_Lee | 1:6228de50cbf4 | 250 | HallB_1_state = HallB.read(); |
Andy_Lee | 1:6228de50cbf4 | 251 | |
Andy_Lee | 1:6228de50cbf4 | 252 | ///code for state determination/// |
Andy_Lee | 1:6228de50cbf4 | 253 | if(state_1==1 && state_1_old==1){ |
Andy_Lee | 1:6228de50cbf4 | 254 | if(HallA_1_state==1 && HallB_1_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 255 | speed_count_1=speed_count_1 + 1; |
Andy_Lee | 1:6228de50cbf4 | 256 | } |
Andy_Lee | 1:6228de50cbf4 | 257 | else if(HallA_1_state==0 && HallB_1_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 258 | speed_count_1=speed_count_1 - 1; |
Andy_Lee | 1:6228de50cbf4 | 259 | } |
Andy_Lee | 1:6228de50cbf4 | 260 | } |
Andy_Lee | 1:6228de50cbf4 | 261 | if(state_1==1 && state_1_old==0){ |
Andy_Lee | 1:6228de50cbf4 | 262 | if(HallA_1_state==0 && HallB_1_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 263 | speed_count_1=speed_count_1 + 1; |
Andy_Lee | 1:6228de50cbf4 | 264 | } |
Andy_Lee | 1:6228de50cbf4 | 265 | else if(HallA_1_state==1 && HallB_1_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 266 | speed_count_1=speed_count_1 - 1; |
Andy_Lee | 1:6228de50cbf4 | 267 | } |
Andy_Lee | 1:6228de50cbf4 | 268 | } |
Andy_Lee | 1:6228de50cbf4 | 269 | if(state_1==0 && state_1_old==0){ |
Andy_Lee | 1:6228de50cbf4 | 270 | if(HallA_1_state==0 && HallB_1_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 271 | speed_count_1=speed_count_1 + 1; |
Andy_Lee | 1:6228de50cbf4 | 272 | } |
Andy_Lee | 1:6228de50cbf4 | 273 | else if(HallA_1_state==1 && HallB_1_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 274 | speed_count_1=speed_count_1 - 1; |
Andy_Lee | 1:6228de50cbf4 | 275 | } |
Andy_Lee | 1:6228de50cbf4 | 276 | } |
Andy_Lee | 1:6228de50cbf4 | 277 | if(state_1==0 && state_1_old==1){ |
Andy_Lee | 1:6228de50cbf4 | 278 | if(HallA_1_state==1 && HallB_1_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 279 | speed_count_1=speed_count_1 + 1; |
Andy_Lee | 1:6228de50cbf4 | 280 | } |
Andy_Lee | 1:6228de50cbf4 | 281 | else if(HallA_1_state==0 && HallB_1_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 282 | speed_count_1=speed_count_1 - 1; |
Andy_Lee | 1:6228de50cbf4 | 283 | } |
Andy_Lee | 1:6228de50cbf4 | 284 | } |
Andy_Lee | 1:6228de50cbf4 | 285 | state_1=HallA_1_state; |
Andy_Lee | 1:6228de50cbf4 | 286 | state_1_old=HallB_1_state; |
Andy_Lee | 1:6228de50cbf4 | 287 | ////////////////////////////////// |
Andy_Lee | 1:6228de50cbf4 | 288 | |
Andy_Lee | 1:6228de50cbf4 | 289 | //forward : speed_count_1 + 1 |
Andy_Lee | 1:6228de50cbf4 | 290 | //backward : speed_count_1 - 1 |
Andy_Lee | 1:6228de50cbf4 | 291 | |
Andy_Lee | 1:6228de50cbf4 | 292 | |
Andy_Lee | 1:6228de50cbf4 | 293 | // motor2 |
Andy_Lee | 1:6228de50cbf4 | 294 | HallA_2_state = HallA_2.read(); |
Andy_Lee | 1:6228de50cbf4 | 295 | HallB_2_state = HallB_2.read(); |
Andy_Lee | 1:6228de50cbf4 | 296 | |
Andy_Lee | 1:6228de50cbf4 | 297 | ///code for state determination/// |
Andy_Lee | 1:6228de50cbf4 | 298 | if(state_2==1 && state_2_old==1){ |
Andy_Lee | 1:6228de50cbf4 | 299 | if(HallA_2_state==1 && HallB_2_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 300 | speed_count_2=speed_count_2 + 1; |
Andy_Lee | 1:6228de50cbf4 | 301 | } |
Andy_Lee | 1:6228de50cbf4 | 302 | else if(HallA_2_state==0 && HallB_2_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 303 | speed_count_2=speed_count_2 - 1; |
Andy_Lee | 1:6228de50cbf4 | 304 | } |
Andy_Lee | 1:6228de50cbf4 | 305 | } |
Andy_Lee | 1:6228de50cbf4 | 306 | if(state_2==1 && state_2_old==0){ |
Andy_Lee | 1:6228de50cbf4 | 307 | if(HallA_2_state==0 && HallB_2_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 308 | speed_count_2=speed_count_2 + 1; |
Andy_Lee | 1:6228de50cbf4 | 309 | } |
Andy_Lee | 1:6228de50cbf4 | 310 | else if(HallA_2_state==1 && HallB_2_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 311 | speed_count_2=speed_count_2 - 1; |
Andy_Lee | 1:6228de50cbf4 | 312 | } |
Andy_Lee | 1:6228de50cbf4 | 313 | } |
Andy_Lee | 1:6228de50cbf4 | 314 | if(state_2==0 && state_2_old==0){ |
Andy_Lee | 1:6228de50cbf4 | 315 | if(HallA_2_state==0 && HallB_2_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 316 | speed_count_2=speed_count_2 + 1; |
Andy_Lee | 1:6228de50cbf4 | 317 | } |
Andy_Lee | 1:6228de50cbf4 | 318 | else if(HallA_2_state==1 && HallB_2_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 319 | speed_count_2=speed_count_2 - 1; |
Andy_Lee | 1:6228de50cbf4 | 320 | } |
Andy_Lee | 1:6228de50cbf4 | 321 | } |
Andy_Lee | 1:6228de50cbf4 | 322 | if(state_2==0 && state_2_old==1){ |
Andy_Lee | 1:6228de50cbf4 | 323 | if(HallA_2_state==1 && HallB_2_state==1){ |
Andy_Lee | 1:6228de50cbf4 | 324 | speed_count_2=speed_count_2 + 1; |
Andy_Lee | 1:6228de50cbf4 | 325 | } |
Andy_Lee | 1:6228de50cbf4 | 326 | else if(HallA_2_state==0 && HallB_2_state==0){ |
Andy_Lee | 1:6228de50cbf4 | 327 | speed_count_2=speed_count_2 - 1; |
Andy_Lee | 1:6228de50cbf4 | 328 | } |
Andy_Lee | 1:6228de50cbf4 | 329 | } |
Andy_Lee | 1:6228de50cbf4 | 330 | state_2=HallA_2_state; |
Andy_Lee | 1:6228de50cbf4 | 331 | state_2_old=HallB_2_state; |
Andy_Lee | 1:6228de50cbf4 | 332 | |
Andy_Lee | 1:6228de50cbf4 | 333 | |
Andy_Lee | 1:6228de50cbf4 | 334 | ////////////////////////////////// |
Andy_Lee | 1:6228de50cbf4 | 335 | |
Andy_Lee | 1:6228de50cbf4 | 336 | //forward : speed_count_2 + 1 |
Andy_Lee | 1:6228de50cbf4 | 337 | //backward : speed_count_2 - 1 |
Andy_Lee | 1:6228de50cbf4 | 338 | } |