No.9 Robotics / Mbed 2 deprecated Robotics_DCMotor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Robotics_Lab_DCMotor by LDSC_Robotics

main.cpp@9:85cbef9febe7, 2016-05-18 (annotated)

Committer:
winstonkuo
Date:
Wed May 18 08:03:32 2016 +0000
Revision:
9:85cbef9febe7
Parent:
6:da43b63ffb24
Child:
11:86912704fc2c
5/18;

Who changed what in which revision?

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