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@6:da43b63ffb24, 2016-04-14 (annotated)

Committer:
ChangYuHsuan
Date:
Thu Apr 14 09:29:00 2016 +0000
Revision:
6:da43b63ffb24
Parent:
5:ff3b5f31a9ce
Child:
9:85cbef9febe7
Child:
10:7954ec17021d
Yu Hsuan;

Who changed what in which revision?

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