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@11:86912704fc2c, 2016-05-20 (annotated)

Committer:
ChangYuHsuan
Date:
Fri May 20 07:01:48 2016 +0000
Revision:
11:86912704fc2c
Parent:
9:85cbef9febe7 
control by matlab; PI digitalization

Who changed what in which revision?

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