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@2:dc39c3de56c1, 2016-05-27 (annotated)

Committer:
jk41126
Date:
Fri May 27 02:37:20 2016 +0000
Revision:
2:dc39c3de56c1
Parent:
1:8fb27fda4ec5
Child:
3:e4bdbe286f7a
789;

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