Robotic_winteam / Mbed 2 deprecated Nucleo_all

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

main.cpp@3:e4bdbe286f7a, 2016-06-10 (annotated)

Committer:
jk41126
Date:
Fri Jun 10 14:32:13 2016 +0000
Revision:
3:e4bdbe286f7a
Parent:
2:dc39c3de56c1 
123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jk41126 0:81b42d622dc5 1 #include "mbed.h"
jk41126 0:81b42d622dc5 2
jk41126 0:81b42d622dc5 3 #define Ts 0.01f //period of timer1 (s)
jk41126 0:81b42d622dc5 4 #define Kp 0.006f
jk41126 0:81b42d622dc5 5 #define Ki 0.02f
jk41126 0:81b42d622dc5 6
jk41126 3:e4bdbe286f7a 7 PwmOut mypwm(A0);
jk41126 0:81b42d622dc5 8 PwmOut pwm1(D7);
jk41126 0:81b42d622dc5 9 PwmOut pwm1n(D11);
jk41126 0:81b42d622dc5 10 PwmOut pwm2(D8);
jk41126 0:81b42d622dc5 11 PwmOut pwm2n(A3);
jk41126 0:81b42d622dc5 12
jk41126 0:81b42d622dc5 13 DigitalOut led1(A4);
jk41126 0:81b42d622dc5 14 DigitalOut led2(A5);
jk41126 0:81b42d622dc5 15
jk41126 0:81b42d622dc5 16 //Motor1 sensor
jk41126 0:81b42d622dc5 17 InterruptIn HallA_1(A1);
jk41126 0:81b42d622dc5 18 InterruptIn HallB_1(A2);
jk41126 0:81b42d622dc5 19 //Motor2 sensor
jk41126 0:81b42d622dc5 20 InterruptIn HallA_2(D13);
jk41126 0:81b42d622dc5 21 InterruptIn HallB_2(D12);
jk41126 0:81b42d622dc5 22
jk41126 0:81b42d622dc5 23 Serial bluetooth (D10,D2);
jk41126 0:81b42d622dc5 24 Serial pc(D1,D0);
jk41126 0:81b42d622dc5 25
jk41126 0:81b42d622dc5 26 Ticker timer1;
jk41126 0:81b42d622dc5 27 void timer1_interrupt(void);
jk41126 0:81b42d622dc5 28 void CN_interrupt(void);
jk41126 0:81b42d622dc5 29 void BT_interrupt(void);
jk41126 0:81b42d622dc5 30
jk41126 0:81b42d622dc5 31 void init_TIMER(void);
jk41126 0:81b42d622dc5 32 void init_PWM(void);
jk41126 0:81b42d622dc5 33 void init_CN(void);
jk41126 0:81b42d622dc5 34
jk41126 0:81b42d622dc5 35 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
jk41126 0:81b42d622dc5 36 int8_t stateA_1_old = 0, stateB_1_old = 0, stateA_2_old = 0, stateB_2_old = 0;
jk41126 0:81b42d622dc5 37
jk41126 0:81b42d622dc5 38 int c = 0;
jk41126 0:81b42d622dc5 39 int d = 0;
jk41126 0:81b42d622dc5 40 int v1Count = 0;
jk41126 0:81b42d622dc5 41 int v2Count = 0;
jk41126 0:81b42d622dc5 42
jk41126 0:81b42d622dc5 43 /*char Receive_Data;*/
jk41126 2:dc39c3de56c1 44 char Receive_Data[10],Temp;
jk41126 2:dc39c3de56c1 45 float Position_Error = 0.0,Degree_Error= 0.0;
jk41126 2:dc39c3de56c1 46 int Command_Flag,Receive_Counter,Receive_Flag;
jk41126 2:dc39c3de56c1 47 float sigma = 0.0,delta = 0.0,fSpeedRef_1 = 0.0,fSpeedRef_2 = 0.0;
jk41126 0:81b42d622dc5 48
jk41126 0:81b42d622dc5 49
jk41126 0:81b42d622dc5 50 float v1 = 0.0, v1_ref = 0.0;
jk41126 0:81b42d622dc5 51 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
jk41126 0:81b42d622dc5 52 float v2 = 0.0, v2_ref = 0.0;
jk41126 0:81b42d622dc5 53 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
jk41126 0:81b42d622dc5 54
jk41126 3:e4bdbe286f7a 55 float k1 = 0.0, k2 = 0.0, servo_duty = 0.121;
jk41126 3:e4bdbe286f7a 56
jk41126 0:81b42d622dc5 57 int main()
jk41126 0:81b42d622dc5 58 {
jk41126 0:81b42d622dc5 59 init_TIMER();
jk41126 0:81b42d622dc5 60 init_PWM();
jk41126 0:81b42d622dc5 61 init_CN();
jk41126 0:81b42d622dc5 62
jk41126 0:81b42d622dc5 63 bluetooth.baud(115200);
jk41126 0:81b42d622dc5 64 pc.baud(57600);
jk41126 1:8fb27fda4ec5 65 bluetooth.attach(&BT_interrupt);
jk41126 0:81b42d622dc5 66
jk41126 0:81b42d622dc5 67 while(1)
jk41126 0:81b42d622dc5 68 {
jk41126 1:8fb27fda4ec5 69 if(Command_Flag==1)
jk41126 0:81b42d622dc5 70 {
jk41126 1:8fb27fda4ec5 71 Position_Error=(Receive_Data[2]-0x30)*100+(Receive_Data[3]-0x30)*10+(Receive_Data[4]-0x30);
jk41126 1:8fb27fda4ec5 72
jk41126 1:8fb27fda4ec5 73 if(Receive_Data[1]=='-')
jk41126 1:8fb27fda4ec5 74 Position_Error=-1*Position_Error;
jk41126 1:8fb27fda4ec5 75 else
jk41126 1:8fb27fda4ec5 76 Position_Error=Position_Error;
jk41126 0:81b42d622dc5 77
jk41126 1:8fb27fda4ec5 78 Degree_Error=(Receive_Data[6]-0x30)*100+(Receive_Data[7]-0x30)*10+(Receive_Data[8]-0x30);
jk41126 1:8fb27fda4ec5 79
jk41126 1:8fb27fda4ec5 80 if(Receive_Data[5]=='-')
jk41126 1:8fb27fda4ec5 81 Degree_Error=-1*Degree_Error;
jk41126 1:8fb27fda4ec5 82 else
jk41126 1:8fb27fda4ec5 83 Degree_Error=Degree_Error;
jk41126 2:dc39c3de56c1 84
jk41126 3:e4bdbe286f7a 85 if(Receive_Data[9]=='j')
jk41126 3:e4bdbe286f7a 86 mypwm.write(0.079);
jk41126 3:e4bdbe286f7a 87 else
jk41126 3:e4bdbe286f7a 88 mypwm.write(0.1257);
jk41126 3:e4bdbe286f7a 89
jk41126 1:8fb27fda4ec5 90
jk41126 3:e4bdbe286f7a 91 if (Position_Error >= 12 || Position_Error <= -12)
jk41126 3:e4bdbe286f7a 92 k1 = 2;
jk41126 3:e4bdbe286f7a 93 else
jk41126 3:e4bdbe286f7a 94 k1 = 1;
jk41126 3:e4bdbe286f7a 95
jk41126 3:e4bdbe286f7a 96 if (Degree_Error >= 15 || Degree_Error <= -15)
jk41126 3:e4bdbe286f7a 97 k2 = 1;
jk41126 3:e4bdbe286f7a 98 else
jk41126 3:e4bdbe286f7a 99 k2 = 0.5;
jk41126 3:e4bdbe286f7a 100
jk41126 3:e4bdbe286f7a 101
jk41126 1:8fb27fda4ec5 102 sigma = k1*Position_Error;
jk41126 1:8fb27fda4ec5 103 delta = k2*Degree_Error;
jk41126 1:8fb27fda4ec5 104
jk41126 1:8fb27fda4ec5 105 if (sigma > 500){sigma = 500;}
jk41126 1:8fb27fda4ec5 106 if (sigma < -500){sigma = -500;}
jk41126 1:8fb27fda4ec5 107 if (delta > 500){delta = 500;}
jk41126 1:8fb27fda4ec5 108 if (delta < -500){delta = -500;}
jk41126 3:e4bdbe286f7a 109
jk41126 3:e4bdbe286f7a 110
jk41126 1:8fb27fda4ec5 111 fSpeedRef_1 = sigma + delta;
jk41126 1:8fb27fda4ec5 112 fSpeedRef_2 = sigma - delta;
jk41126 1:8fb27fda4ec5 113 v1_ref = fSpeedRef_1;
jk41126 1:8fb27fda4ec5 114 v2_ref = -1*fSpeedRef_2;
jk41126 3:e4bdbe286f7a 115
jk41126 3:e4bdbe286f7a 116 Command_Flag = 0;
jk41126 0:81b42d622dc5 117 }
jk41126 0:81b42d622dc5 118 }
jk41126 0:81b42d622dc5 119 }
jk41126 0:81b42d622dc5 120
jk41126 0:81b42d622dc5 121 void timer1_interrupt(void)
jk41126 0:81b42d622dc5 122 {
jk41126 0:81b42d622dc5 123 //Motor 1
jk41126 0:81b42d622dc5 124 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
jk41126 0:81b42d622dc5 125 v1Count = 0;
jk41126 0:81b42d622dc5 126
jk41126 0:81b42d622dc5 127 ///code for PI control///
jk41126 0:81b42d622dc5 128
jk41126 0:81b42d622dc5 129 v1_err = v1_ref - v1;
jk41126 0:81b42d622dc5 130 v1_ierr = v1_ierr + v1_err*Ts;
jk41126 0:81b42d622dc5 131 PIout_1 = Kp*v1_err + Ki*v1_ierr;
jk41126 0:81b42d622dc5 132
jk41126 0:81b42d622dc5 133 /////////////////////////
jk41126 0:81b42d622dc5 134
jk41126 0:81b42d622dc5 135 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
jk41126 0:81b42d622dc5 136 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
jk41126 0:81b42d622dc5 137 pwm1.write(PIout_1 + 0.5f);
jk41126 0:81b42d622dc5 138 TIM1->CCER |= 0x4;
jk41126 0:81b42d622dc5 139
jk41126 0:81b42d622dc5 140
jk41126 0:81b42d622dc5 141 //Motor 2
jk41126 0:81b42d622dc5 142 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
jk41126 0:81b42d622dc5 143 v2Count = 0;
jk41126 0:81b42d622dc5 144
jk41126 0:81b42d622dc5 145 ///code for PI control///
jk41126 0:81b42d622dc5 146
jk41126 0:81b42d622dc5 147 v2_err = v2_ref - v2;
jk41126 0:81b42d622dc5 148 v2_ierr = v2_ierr + v2_err*Ts;
jk41126 0:81b42d622dc5 149 PIout_2 = Kp*v2_err + Ki*v2_ierr;
jk41126 0:81b42d622dc5 150
jk41126 0:81b42d622dc5 151 /////////////////////////
jk41126 0:81b42d622dc5 152
jk41126 0:81b42d622dc5 153 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
jk41126 0:81b42d622dc5 154 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
jk41126 0:81b42d622dc5 155 pwm2.write(PIout_2 + 0.5f);
jk41126 0:81b42d622dc5 156 TIM1->CCER |= 0x40;
jk41126 0:81b42d622dc5 157
jk41126 0:81b42d622dc5 158
jk41126 0:81b42d622dc5 159 }
jk41126 0:81b42d622dc5 160
jk41126 0:81b42d622dc5 161 void CN_interrupt(void)
jk41126 0:81b42d622dc5 162 {
jk41126 0:81b42d622dc5 163 //Motor 1
jk41126 0:81b42d622dc5 164 stateA_1 = HallA_1.read();
jk41126 0:81b42d622dc5 165 stateB_1 = HallB_1.read();
jk41126 0:81b42d622dc5 166
jk41126 0:81b42d622dc5 167 ///code for state determination///
jk41126 0:81b42d622dc5 168
jk41126 0:81b42d622dc5 169 if(stateA_1 == stateB_1_old && c == 1)
jk41126 0:81b42d622dc5 170 {
jk41126 0:81b42d622dc5 171 v1Count = v1Count + 1;
jk41126 0:81b42d622dc5 172 }
jk41126 0:81b42d622dc5 173
jk41126 0:81b42d622dc5 174 if(stateB_1 == stateA_1_old && c == 1)
jk41126 0:81b42d622dc5 175 {
jk41126 0:81b42d622dc5 176 v1Count = v1Count - 1;
jk41126 0:81b42d622dc5 177 }
jk41126 0:81b42d622dc5 178
jk41126 0:81b42d622dc5 179 c = 1;
jk41126 0:81b42d622dc5 180 stateA_1_old = stateA_1;
jk41126 0:81b42d622dc5 181 stateB_1_old = stateB_1;
jk41126 0:81b42d622dc5 182
jk41126 0:81b42d622dc5 183 //////////////////////////////////
jk41126 0:81b42d622dc5 184
jk41126 0:81b42d622dc5 185
jk41126 0:81b42d622dc5 186 //Motor 2
jk41126 0:81b42d622dc5 187 stateA_2 = HallA_2.read();
jk41126 0:81b42d622dc5 188 stateB_2 = HallB_2.read();
jk41126 0:81b42d622dc5 189
jk41126 0:81b42d622dc5 190 ///code for state determination///
jk41126 0:81b42d622dc5 191
jk41126 0:81b42d622dc5 192 if(stateA_2 == stateB_2_old && d == 1)
jk41126 0:81b42d622dc5 193 {
jk41126 0:81b42d622dc5 194 v2Count = v2Count + 1;
jk41126 0:81b42d622dc5 195 }
jk41126 0:81b42d622dc5 196
jk41126 0:81b42d622dc5 197 if(stateB_2 == stateA_2_old && d == 1)
jk41126 0:81b42d622dc5 198 {
jk41126 0:81b42d622dc5 199 v2Count = v2Count - 1;
jk41126 0:81b42d622dc5 200 }
jk41126 0:81b42d622dc5 201
jk41126 0:81b42d622dc5 202 d = 1;
jk41126 0:81b42d622dc5 203 stateA_2_old = stateA_2;
jk41126 0:81b42d622dc5 204 stateB_2_old = stateB_2;
jk41126 0:81b42d622dc5 205
jk41126 0:81b42d622dc5 206 //////////////////////////////////
jk41126 0:81b42d622dc5 207
jk41126 0:81b42d622dc5 208 }
jk41126 0:81b42d622dc5 209
jk41126 0:81b42d622dc5 210 void init_TIMER(void)
jk41126 0:81b42d622dc5 211 {
jk41126 0:81b42d622dc5 212 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
jk41126 0:81b42d622dc5 213 }
jk41126 0:81b42d622dc5 214
jk41126 0:81b42d622dc5 215 void init_PWM(void)
jk41126 0:81b42d622dc5 216 {
jk41126 0:81b42d622dc5 217 pwm1.period_us(50);
jk41126 0:81b42d622dc5 218 pwm1.write(0.5);
jk41126 0:81b42d622dc5 219 TIM1->CCER |= 0x4;
jk41126 0:81b42d622dc5 220
jk41126 0:81b42d622dc5 221 pwm2.period_us(50);
jk41126 0:81b42d622dc5 222 pwm2.write(0.5);
jk41126 0:81b42d622dc5 223 TIM1->CCER |= 0x40;
jk41126 3:e4bdbe286f7a 224
jk41126 3:e4bdbe286f7a 225 mypwm.period_ms(20);
jk41126 3:e4bdbe286f7a 226 mypwm.write(servo_duty);
jk41126 0:81b42d622dc5 227 }
jk41126 0:81b42d622dc5 228
jk41126 0:81b42d622dc5 229 void init_CN(void)
jk41126 0:81b42d622dc5 230 {
jk41126 0:81b42d622dc5 231 HallA_1.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 232 HallA_1.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 233 HallB_1.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 234 HallB_1.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 235
jk41126 0:81b42d622dc5 236 HallA_2.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 237 HallA_2.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 238 HallB_2.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 239 HallB_2.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 240
jk41126 0:81b42d622dc5 241 stateA_1 = HallA_1.read();
jk41126 0:81b42d622dc5 242 stateB_1 = HallB_1.read();
jk41126 0:81b42d622dc5 243 stateA_2 = HallA_2.read();
jk41126 0:81b42d622dc5 244 stateB_2 = HallB_2.read();
jk41126 0:81b42d622dc5 245 }
jk41126 0:81b42d622dc5 246
jk41126 0:81b42d622dc5 247 void BT_interrupt(void)
jk41126 2:dc39c3de56c1 248 {
jk41126 3:e4bdbe286f7a 249 if(bluetooth.readable())
jk41126 0:81b42d622dc5 250 {
jk41126 3:e4bdbe286f7a 251 Temp = bluetooth.getc();
jk41126 3:e4bdbe286f7a 252
jk41126 3:e4bdbe286f7a 253 if (Receive_Flag==1)
jk41126 0:81b42d622dc5 254 {
jk41126 3:e4bdbe286f7a 255 Receive_Counter++;
jk41126 3:e4bdbe286f7a 256 Receive_Data[Receive_Counter]=Temp;
jk41126 3:e4bdbe286f7a 257
jk41126 3:e4bdbe286f7a 258 if(Receive_Counter==9)
jk41126 3:e4bdbe286f7a 259 {
jk41126 3:e4bdbe286f7a 260 Command_Flag=1;
jk41126 3:e4bdbe286f7a 261 Receive_Flag=0;
jk41126 3:e4bdbe286f7a 262 Receive_Counter=0;
jk41126 3:e4bdbe286f7a 263 }
jk41126 0:81b42d622dc5 264 }
jk41126 3:e4bdbe286f7a 265 else
jk41126 0:81b42d622dc5 266 {
jk41126 3:e4bdbe286f7a 267 if(Temp == 36)
jk41126 3:e4bdbe286f7a 268 {
jk41126 3:e4bdbe286f7a 269 Receive_Flag = 1;
jk41126 3:e4bdbe286f7a 270 Receive_Counter = 0;
jk41126 3:e4bdbe286f7a 271 Receive_Data[0]= Temp;
jk41126 3:e4bdbe286f7a 272 }
jk41126 0:81b42d622dc5 273 }
jk41126 0:81b42d622dc5 274 }
jk41126 0:81b42d622dc5 275 }