Robotics Term Project / Mbed 2 deprecated speed

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?

UserRevisionLine numberNew 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 }