speed
Dependencies: mbed
main.cpp@0:95df37b1dd11, 2016-05-28 (annotated)
- Committer:
- roger5641
- Date:
- Sat May 28 12:50:40 2016 +0000
- Revision:
- 0:95df37b1dd11
speed;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roger5641 | 0:95df37b1dd11 | 1 | /*DCMotor*/ |
roger5641 | 0:95df37b1dd11 | 2 | // transfer function 10400/s+21.28 |
roger5641 | 0:95df37b1dd11 | 3 | // motor direction v1(vR) < 0 |
roger5641 | 0:95df37b1dd11 | 4 | // motor direction v2(vL) > 0 |
roger5641 | 0:95df37b1dd11 | 5 | // speed limitatino 300 rpm |
roger5641 | 0:95df37b1dd11 | 6 | |
roger5641 | 0:95df37b1dd11 | 7 | #include "mbed.h" |
roger5641 | 0:95df37b1dd11 | 8 | |
roger5641 | 0:95df37b1dd11 | 9 | //The number will be compiled as type "double" in default |
roger5641 | 0:95df37b1dd11 | 10 | //Add a "f" after the number can make it compiled as type "float" |
roger5641 | 0:95df37b1dd11 | 11 | #define Ts 0.02f //period of timer1 (s) |
roger5641 | 0:95df37b1dd11 | 12 | #define Kp1 0.0048f //0.0048f |
roger5641 | 0:95df37b1dd11 | 13 | #define Kp2 0.0048f //0.0048f |
roger5641 | 0:95df37b1dd11 | 14 | #define Ki 0.002754f //0.1023f |
roger5641 | 0:95df37b1dd11 | 15 | |
roger5641 | 0:95df37b1dd11 | 16 | Serial bluetooth(D10,D2); //宣告藍牙腳位 |
roger5641 | 0:95df37b1dd11 | 17 | //Serial pc(D1, D0); |
roger5641 | 0:95df37b1dd11 | 18 | PwmOut servo(A0); |
roger5641 | 0:95df37b1dd11 | 19 | PwmOut pwm1(D7); |
roger5641 | 0:95df37b1dd11 | 20 | PwmOut pwm1n(D11); |
roger5641 | 0:95df37b1dd11 | 21 | PwmOut pwm2(D8); |
roger5641 | 0:95df37b1dd11 | 22 | PwmOut pwm2n(A3); |
roger5641 | 0:95df37b1dd11 | 23 | |
roger5641 | 0:95df37b1dd11 | 24 | DigitalOut led1(A4); |
roger5641 | 0:95df37b1dd11 | 25 | DigitalOut led2(A5); |
roger5641 | 0:95df37b1dd11 | 26 | |
roger5641 | 0:95df37b1dd11 | 27 | //Motor1 sensor |
roger5641 | 0:95df37b1dd11 | 28 | InterruptIn HallA_1(A1); |
roger5641 | 0:95df37b1dd11 | 29 | InterruptIn HallB_1(A2); |
roger5641 | 0:95df37b1dd11 | 30 | //Motor2 sensor |
roger5641 | 0:95df37b1dd11 | 31 | InterruptIn HallA_2(D13); |
roger5641 | 0:95df37b1dd11 | 32 | InterruptIn HallB_2(D12); |
roger5641 | 0:95df37b1dd11 | 33 | |
roger5641 | 0:95df37b1dd11 | 34 | |
roger5641 | 0:95df37b1dd11 | 35 | Ticker timer1; |
roger5641 | 0:95df37b1dd11 | 36 | void timer1_interrupt(void); |
roger5641 | 0:95df37b1dd11 | 37 | int timer1_counter; |
roger5641 | 0:95df37b1dd11 | 38 | |
roger5641 | 0:95df37b1dd11 | 39 | void CN_interrupt(void); |
roger5641 | 0:95df37b1dd11 | 40 | |
roger5641 | 0:95df37b1dd11 | 41 | void init_TIMER(void); |
roger5641 | 0:95df37b1dd11 | 42 | void init_PWM(void); |
roger5641 | 0:95df37b1dd11 | 43 | void init_CN(void); |
roger5641 | 0:95df37b1dd11 | 44 | void init_BLUETOOTH(void); |
roger5641 | 0:95df37b1dd11 | 45 | |
roger5641 | 0:95df37b1dd11 | 46 | int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0; |
roger5641 | 0:95df37b1dd11 | 47 | int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0; |
roger5641 | 0:95df37b1dd11 | 48 | |
roger5641 | 0:95df37b1dd11 | 49 | int v1Count = 0; |
roger5641 | 0:95df37b1dd11 | 50 | int v2Count = 0; |
roger5641 | 0:95df37b1dd11 | 51 | |
roger5641 | 0:95df37b1dd11 | 52 | float v1 = 0.0, v1_ref = 0.0; |
roger5641 | 0:95df37b1dd11 | 53 | float v1_err = 0.0, v1_err_old = 0.0, PIout_1 = 0.0; |
roger5641 | 0:95df37b1dd11 | 54 | float v1_old[10] = {}, v1_avg = 0.0; |
roger5641 | 0:95df37b1dd11 | 55 | float v2 = 0.0, v2_ref = 0.0; |
roger5641 | 0:95df37b1dd11 | 56 | float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0; |
roger5641 | 0:95df37b1dd11 | 57 | float v2_old[10] = {}, v2_avg = 0.0; |
roger5641 | 0:95df37b1dd11 | 58 | |
roger5641 | 0:95df37b1dd11 | 59 | int servo_angle = 87; |
roger5641 | 0:95df37b1dd11 | 60 | float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90 |
roger5641 | 0:95df37b1dd11 | 61 | |
roger5641 | 0:95df37b1dd11 | 62 | char Receive_Data[8] = {}; |
roger5641 | 0:95df37b1dd11 | 63 | |
roger5641 | 0:95df37b1dd11 | 64 | int main() { |
roger5641 | 0:95df37b1dd11 | 65 | init_BLUETOOTH(); |
roger5641 | 0:95df37b1dd11 | 66 | init_TIMER(); |
roger5641 | 0:95df37b1dd11 | 67 | init_PWM(); |
roger5641 | 0:95df37b1dd11 | 68 | init_CN(); |
roger5641 | 0:95df37b1dd11 | 69 | |
roger5641 | 0:95df37b1dd11 | 70 | v1_ref = 0.0; |
roger5641 | 0:95df37b1dd11 | 71 | v2_ref = 0.0; |
roger5641 | 0:95df37b1dd11 | 72 | |
roger5641 | 0:95df37b1dd11 | 73 | //bluetooth.baud(115200); //設定鮑率 |
roger5641 | 0:95df37b1dd11 | 74 | //pc.baud(57600); |
roger5641 | 0:95df37b1dd11 | 75 | |
roger5641 | 0:95df37b1dd11 | 76 | while(1) |
roger5641 | 0:95df37b1dd11 | 77 | { |
roger5641 | 0:95df37b1dd11 | 78 | if(bluetooth.readable()) |
roger5641 | 0:95df37b1dd11 | 79 | { |
roger5641 | 0:95df37b1dd11 | 80 | bluetooth.scanf("%f%f",&v1_ref,&v2_ref); |
roger5641 | 0:95df37b1dd11 | 81 | v1_ref = v1_ref - 300.0f; |
roger5641 | 0:95df37b1dd11 | 82 | v2_ref = v2_ref - 300.0f; |
roger5641 | 0:95df37b1dd11 | 83 | } |
roger5641 | 0:95df37b1dd11 | 84 | /*if(pc.readable()) |
roger5641 | 0:95df37b1dd11 | 85 | { |
roger5641 | 0:95df37b1dd11 | 86 | bluetooth.putc(pc.getc()); |
roger5641 | 0:95df37b1dd11 | 87 | } |
roger5641 | 0:95df37b1dd11 | 88 | if(bluetooth.readable()) |
roger5641 | 0:95df37b1dd11 | 89 | { |
roger5641 | 0:95df37b1dd11 | 90 | pc.putc(bluetooth.getc()); |
roger5641 | 0:95df37b1dd11 | 91 | }*/ |
roger5641 | 0:95df37b1dd11 | 92 | } |
roger5641 | 0:95df37b1dd11 | 93 | } |
roger5641 | 0:95df37b1dd11 | 94 | |
roger5641 | 0:95df37b1dd11 | 95 | void timer1_interrupt(void) |
roger5641 | 0:95df37b1dd11 | 96 | { |
roger5641 | 0:95df37b1dd11 | 97 | |
roger5641 | 0:95df37b1dd11 | 98 | /*for(int i=0; i<8; i++) |
roger5641 | 0:95df37b1dd11 | 99 | { |
roger5641 | 0:95df37b1dd11 | 100 | Receive_Data[i] = bluetooth.getc(); |
roger5641 | 0:95df37b1dd11 | 101 | } |
roger5641 | 0:95df37b1dd11 | 102 | //read data from matlab |
roger5641 | 0:95df37b1dd11 | 103 | //distance_target |
roger5641 | 0:95df37b1dd11 | 104 | v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30); |
roger5641 | 0:95df37b1dd11 | 105 | |
roger5641 | 0:95df37b1dd11 | 106 | if(Receive_Data[0] == '-')v1_ref = -1* v1_ref; |
roger5641 | 0:95df37b1dd11 | 107 | else v1_ref = v1_ref; |
roger5641 | 0:95df37b1dd11 | 108 | |
roger5641 | 0:95df37b1dd11 | 109 | //ang_rel_target |
roger5641 | 0:95df37b1dd11 | 110 | v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30); |
roger5641 | 0:95df37b1dd11 | 111 | |
roger5641 | 0:95df37b1dd11 | 112 | if(Receive_Data[4] == '-')v2_ref = -1* v2_ref; |
roger5641 | 0:95df37b1dd11 | 113 | else v2_ref = v2_ref;*/ |
roger5641 | 0:95df37b1dd11 | 114 | |
roger5641 | 0:95df37b1dd11 | 115 | //Motor 1 |
roger5641 | 0:95df37b1dd11 | 116 | v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
roger5641 | 0:95df37b1dd11 | 117 | v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f; |
roger5641 | 0:95df37b1dd11 | 118 | for(int i = 9; i > 0 ; i--) |
roger5641 | 0:95df37b1dd11 | 119 | { |
roger5641 | 0:95df37b1dd11 | 120 | v1_old[i] = v1_old[i-1]; |
roger5641 | 0:95df37b1dd11 | 121 | } |
roger5641 | 0:95df37b1dd11 | 122 | v1_old[0] = v1; |
roger5641 | 0:95df37b1dd11 | 123 | v1Count = 0; |
roger5641 | 0:95df37b1dd11 | 124 | |
roger5641 | 0:95df37b1dd11 | 125 | ///code for PI control/// |
roger5641 | 0:95df37b1dd11 | 126 | v1_err = v1_ref - v1; |
roger5641 | 0:95df37b1dd11 | 127 | |
roger5641 | 0:95df37b1dd11 | 128 | PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old; |
roger5641 | 0:95df37b1dd11 | 129 | v1_err_old = v1_err; |
roger5641 | 0:95df37b1dd11 | 130 | |
roger5641 | 0:95df37b1dd11 | 131 | // saturation |
roger5641 | 0:95df37b1dd11 | 132 | if(PIout_1 >= 0.5f)PIout_1 = 0.5f; |
roger5641 | 0:95df37b1dd11 | 133 | else if(PIout_1 <= -0.5f)PIout_1 = -0.5f; |
roger5641 | 0:95df37b1dd11 | 134 | pwm1.write(PIout_1 + 0.5f); |
roger5641 | 0:95df37b1dd11 | 135 | TIM1->CCER |= 0x4; |
roger5641 | 0:95df37b1dd11 | 136 | |
roger5641 | 0:95df37b1dd11 | 137 | //Motor 2 |
roger5641 | 0:95df37b1dd11 | 138 | v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
roger5641 | 0:95df37b1dd11 | 139 | v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f; |
roger5641 | 0:95df37b1dd11 | 140 | for(int i = 9; i > 0; i--) |
roger5641 | 0:95df37b1dd11 | 141 | { |
roger5641 | 0:95df37b1dd11 | 142 | v2_old[i] = v2_old[i-1]; |
roger5641 | 0:95df37b1dd11 | 143 | } |
roger5641 | 0:95df37b1dd11 | 144 | v2_old[0] = v2; |
roger5641 | 0:95df37b1dd11 | 145 | v2Count = 0; |
roger5641 | 0:95df37b1dd11 | 146 | |
roger5641 | 0:95df37b1dd11 | 147 | ///code for PI control/// |
roger5641 | 0:95df37b1dd11 | 148 | v2_err = v2_ref - v2; |
roger5641 | 0:95df37b1dd11 | 149 | |
roger5641 | 0:95df37b1dd11 | 150 | PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old; |
roger5641 | 0:95df37b1dd11 | 151 | v2_err_old = v2_err; |
roger5641 | 0:95df37b1dd11 | 152 | |
roger5641 | 0:95df37b1dd11 | 153 | //saturation |
roger5641 | 0:95df37b1dd11 | 154 | if(PIout_2 >= 0.5f)PIout_2 = 0.5f; |
roger5641 | 0:95df37b1dd11 | 155 | else if(PIout_2 <= -0.5f)PIout_2 = -0.5f; |
roger5641 | 0:95df37b1dd11 | 156 | pwm2.write(PIout_2 + 0.5f); |
roger5641 | 0:95df37b1dd11 | 157 | TIM1->CCER |= 0x40; |
roger5641 | 0:95df37b1dd11 | 158 | |
roger5641 | 0:95df37b1dd11 | 159 | timer1_counter ++; |
roger5641 | 0:95df37b1dd11 | 160 | if (timer1_counter == 50) |
roger5641 | 0:95df37b1dd11 | 161 | { |
roger5641 | 0:95df37b1dd11 | 162 | timer1_counter = 0; |
roger5641 | 0:95df37b1dd11 | 163 | if(bluetooth.writeable()) |
roger5641 | 0:95df37b1dd11 | 164 | { |
roger5641 | 0:95df37b1dd11 | 165 | bluetooth.printf("V1: %4.2f V2: %4.2f\n",v1_avg,v2_avg); |
roger5641 | 0:95df37b1dd11 | 166 | } |
roger5641 | 0:95df37b1dd11 | 167 | } |
roger5641 | 0:95df37b1dd11 | 168 | |
roger5641 | 0:95df37b1dd11 | 169 | /*servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改 |
roger5641 | 0:95df37b1dd11 | 170 | servo.write(servo_duty); |
roger5641 | 0:95df37b1dd11 | 171 | servo = 1; |
roger5641 | 0:95df37b1dd11 | 172 | wait(0.1); |
roger5641 | 0:95df37b1dd11 | 173 | servo = 0; |
roger5641 | 0:95df37b1dd11 | 174 | */ |
roger5641 | 0:95df37b1dd11 | 175 | } |
roger5641 | 0:95df37b1dd11 | 176 | |
roger5641 | 0:95df37b1dd11 | 177 | void CN_interrupt(void) |
roger5641 | 0:95df37b1dd11 | 178 | { |
roger5641 | 0:95df37b1dd11 | 179 | //Motor 1 |
roger5641 | 0:95df37b1dd11 | 180 | stateA_1 = HallA_1.read(); |
roger5641 | 0:95df37b1dd11 | 181 | stateB_1 = HallB_1.read(); |
roger5641 | 0:95df37b1dd11 | 182 | |
roger5641 | 0:95df37b1dd11 | 183 | ///code for state determination/// |
roger5641 | 0:95df37b1dd11 | 184 | if (stateA_1 == 0) |
roger5641 | 0:95df37b1dd11 | 185 | { |
roger5641 | 0:95df37b1dd11 | 186 | if (stateB_1 == 0) |
roger5641 | 0:95df37b1dd11 | 187 | state_1 = 0; |
roger5641 | 0:95df37b1dd11 | 188 | else |
roger5641 | 0:95df37b1dd11 | 189 | state_1 = 1; |
roger5641 | 0:95df37b1dd11 | 190 | } |
roger5641 | 0:95df37b1dd11 | 191 | else |
roger5641 | 0:95df37b1dd11 | 192 | { |
roger5641 | 0:95df37b1dd11 | 193 | if (stateB_1 == 1) |
roger5641 | 0:95df37b1dd11 | 194 | state_1 = 2; |
roger5641 | 0:95df37b1dd11 | 195 | else |
roger5641 | 0:95df37b1dd11 | 196 | state_1 = 3; |
roger5641 | 0:95df37b1dd11 | 197 | } |
roger5641 | 0:95df37b1dd11 | 198 | |
roger5641 | 0:95df37b1dd11 | 199 | //Forward: v1Count +1 |
roger5641 | 0:95df37b1dd11 | 200 | //Inverse: v1Count -1 |
roger5641 | 0:95df37b1dd11 | 201 | if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) ) |
roger5641 | 0:95df37b1dd11 | 202 | v1Count++; |
roger5641 | 0:95df37b1dd11 | 203 | else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0)) |
roger5641 | 0:95df37b1dd11 | 204 | v1Count--; |
roger5641 | 0:95df37b1dd11 | 205 | |
roger5641 | 0:95df37b1dd11 | 206 | state_1_old = state_1; |
roger5641 | 0:95df37b1dd11 | 207 | |
roger5641 | 0:95df37b1dd11 | 208 | //Motor 2 |
roger5641 | 0:95df37b1dd11 | 209 | stateA_2 = HallA_2.read(); |
roger5641 | 0:95df37b1dd11 | 210 | stateB_2 = HallB_2.read(); |
roger5641 | 0:95df37b1dd11 | 211 | |
roger5641 | 0:95df37b1dd11 | 212 | ///code for state determination/// |
roger5641 | 0:95df37b1dd11 | 213 | if (stateA_2 == 0) |
roger5641 | 0:95df37b1dd11 | 214 | { |
roger5641 | 0:95df37b1dd11 | 215 | if (stateB_2 == 0) |
roger5641 | 0:95df37b1dd11 | 216 | state_2 = 0; |
roger5641 | 0:95df37b1dd11 | 217 | else |
roger5641 | 0:95df37b1dd11 | 218 | state_2 = 1; |
roger5641 | 0:95df37b1dd11 | 219 | } |
roger5641 | 0:95df37b1dd11 | 220 | else |
roger5641 | 0:95df37b1dd11 | 221 | { |
roger5641 | 0:95df37b1dd11 | 222 | if (stateB_2 == 1) |
roger5641 | 0:95df37b1dd11 | 223 | state_2 = 2; |
roger5641 | 0:95df37b1dd11 | 224 | else |
roger5641 | 0:95df37b1dd11 | 225 | state_2 = 3; |
roger5641 | 0:95df37b1dd11 | 226 | } |
roger5641 | 0:95df37b1dd11 | 227 | |
roger5641 | 0:95df37b1dd11 | 228 | //Forward: v2Count +1 |
roger5641 | 0:95df37b1dd11 | 229 | //Inverse: v2Count -1 |
roger5641 | 0:95df37b1dd11 | 230 | if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) ) |
roger5641 | 0:95df37b1dd11 | 231 | v2Count++; |
roger5641 | 0:95df37b1dd11 | 232 | else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) ) |
roger5641 | 0:95df37b1dd11 | 233 | v2Count--; |
roger5641 | 0:95df37b1dd11 | 234 | |
roger5641 | 0:95df37b1dd11 | 235 | state_2_old = state_2; |
roger5641 | 0:95df37b1dd11 | 236 | |
roger5641 | 0:95df37b1dd11 | 237 | |
roger5641 | 0:95df37b1dd11 | 238 | } |
roger5641 | 0:95df37b1dd11 | 239 | |
roger5641 | 0:95df37b1dd11 | 240 | void init_TIMER(void) |
roger5641 | 0:95df37b1dd11 | 241 | { |
roger5641 | 0:95df37b1dd11 | 242 | timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz) |
roger5641 | 0:95df37b1dd11 | 243 | timer1_counter = 0; |
roger5641 | 0:95df37b1dd11 | 244 | } |
roger5641 | 0:95df37b1dd11 | 245 | |
roger5641 | 0:95df37b1dd11 | 246 | void init_PWM(void) |
roger5641 | 0:95df37b1dd11 | 247 | { |
roger5641 | 0:95df37b1dd11 | 248 | pwm1.period_us(50); |
roger5641 | 0:95df37b1dd11 | 249 | pwm1.write(0.5); |
roger5641 | 0:95df37b1dd11 | 250 | TIM1->CCER |= 0x4; |
roger5641 | 0:95df37b1dd11 | 251 | |
roger5641 | 0:95df37b1dd11 | 252 | pwm2.period_us(50); |
roger5641 | 0:95df37b1dd11 | 253 | pwm2.write(0.5); |
roger5641 | 0:95df37b1dd11 | 254 | TIM1->CCER |= 0x40; |
roger5641 | 0:95df37b1dd11 | 255 | } |
roger5641 | 0:95df37b1dd11 | 256 | |
roger5641 | 0:95df37b1dd11 | 257 | void init_CN(void) |
roger5641 | 0:95df37b1dd11 | 258 | { |
roger5641 | 0:95df37b1dd11 | 259 | HallA_1.rise(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 260 | HallA_1.fall(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 261 | HallB_1.rise(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 262 | HallB_1.fall(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 263 | |
roger5641 | 0:95df37b1dd11 | 264 | HallA_2.rise(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 265 | HallA_2.fall(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 266 | HallB_2.rise(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 267 | HallB_2.fall(&CN_interrupt); |
roger5641 | 0:95df37b1dd11 | 268 | |
roger5641 | 0:95df37b1dd11 | 269 | stateA_1 = HallA_1.read(); |
roger5641 | 0:95df37b1dd11 | 270 | stateB_1 = HallB_1.read(); |
roger5641 | 0:95df37b1dd11 | 271 | stateA_2 = HallA_2.read(); |
roger5641 | 0:95df37b1dd11 | 272 | stateB_2 = HallB_2.read(); |
roger5641 | 0:95df37b1dd11 | 273 | } |
roger5641 | 0:95df37b1dd11 | 274 | void init_BLUETOOTH(void) |
roger5641 | 0:95df37b1dd11 | 275 | { |
roger5641 | 0:95df37b1dd11 | 276 | bluetooth.baud(115200); |
roger5641 | 0:95df37b1dd11 | 277 | } |