Roger Weng / Mbed 2 deprecated Speed_control

Robotic final work

Dependencies:   mbed

Fork of Speed_control by Robotics Term Project

main.cpp@5:63c9ab496df9, 2016-06-06 (annotated)

Committer:
smilestone520
Date:
Mon Jun 06 10:07:52 2016 +0000
Revision:
5:63c9ab496df9
Parent:
3:f30bd4a0d761
Child:
6:e3cc19965e74
work

Who changed what in which revision?

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