Robotics Term Project / Mbed 2 deprecated Robottics_Motion

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

Speed_Control.cpp@34:b0ae940a2fb5, 2016-05-27 (annotated)

Committer:
smilestone520
Date:
Fri May 27 00:41:22 2016 +0000
Revision:
34:b0ae940a2fb5
hahah

Who changed what in which revision?

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