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@1:8fb27fda4ec5, 2016-05-26 (annotated)

Committer:
jk41126
Date:
Thu May 26 12:54:23 2016 +0000
Revision:
1:8fb27fda4ec5
Parent:
0:81b42d622dc5
Child:
2:dc39c3de56c1
456

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 0:81b42d622dc5 45 char Receive_Data[10];
jk41126 0:81b42d622dc5 46 int Position_Error,Degree_Error;
jk41126 0:81b42d622dc5 47 int Command_Flag;
jk41126 0:81b42d622dc5 48 float sigma,delta,fSpeedRef_1,fSpeedRef_2;
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 1:8fb27fda4ec5 83
jk41126 1:8fb27fda4ec5 84
jk41126 1:8fb27fda4ec5 85 sigma = k1*Position_Error;
jk41126 1:8fb27fda4ec5 86 delta = k2*Degree_Error;
jk41126 1:8fb27fda4ec5 87
jk41126 1:8fb27fda4ec5 88 if (sigma > 500){sigma = 500;}
jk41126 1:8fb27fda4ec5 89 if (sigma < -500){sigma = -500;}
jk41126 1:8fb27fda4ec5 90 if (delta > 500){delta = 500;}
jk41126 1:8fb27fda4ec5 91 if (delta < -500){delta = -500;}
jk41126 1:8fb27fda4ec5 92 fSpeedRef_1 = sigma + delta;
jk41126 1:8fb27fda4ec5 93 fSpeedRef_2 = sigma - delta;
jk41126 1:8fb27fda4ec5 94 v1_ref = fSpeedRef_1;
jk41126 1:8fb27fda4ec5 95 v2_ref = -1*fSpeedRef_2;
jk41126 0:81b42d622dc5 96 }
jk41126 0:81b42d622dc5 97 }
jk41126 0:81b42d622dc5 98 }
jk41126 0:81b42d622dc5 99
jk41126 0:81b42d622dc5 100 void timer1_interrupt(void)
jk41126 0:81b42d622dc5 101 {
jk41126 0:81b42d622dc5 102 //Motor 1
jk41126 0:81b42d622dc5 103 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
jk41126 0:81b42d622dc5 104 v1Count = 0;
jk41126 0:81b42d622dc5 105
jk41126 0:81b42d622dc5 106 ///code for PI control///
jk41126 0:81b42d622dc5 107
jk41126 0:81b42d622dc5 108 v1_err = v1_ref - v1;
jk41126 0:81b42d622dc5 109 v1_ierr = v1_ierr + v1_err*Ts;
jk41126 0:81b42d622dc5 110 PIout_1 = Kp*v1_err + Ki*v1_ierr;
jk41126 0:81b42d622dc5 111
jk41126 0:81b42d622dc5 112 /////////////////////////
jk41126 0:81b42d622dc5 113
jk41126 0:81b42d622dc5 114 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
jk41126 0:81b42d622dc5 115 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
jk41126 0:81b42d622dc5 116 pwm1.write(PIout_1 + 0.5f);
jk41126 0:81b42d622dc5 117 TIM1->CCER |= 0x4;
jk41126 0:81b42d622dc5 118
jk41126 0:81b42d622dc5 119
jk41126 0:81b42d622dc5 120 //Motor 2
jk41126 0:81b42d622dc5 121 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
jk41126 0:81b42d622dc5 122 v2Count = 0;
jk41126 0:81b42d622dc5 123
jk41126 0:81b42d622dc5 124 ///code for PI control///
jk41126 0:81b42d622dc5 125
jk41126 0:81b42d622dc5 126 v2_err = v2_ref - v2;
jk41126 0:81b42d622dc5 127 v2_ierr = v2_ierr + v2_err*Ts;
jk41126 0:81b42d622dc5 128 PIout_2 = Kp*v2_err + Ki*v2_ierr;
jk41126 0:81b42d622dc5 129
jk41126 0:81b42d622dc5 130 /////////////////////////
jk41126 0:81b42d622dc5 131
jk41126 0:81b42d622dc5 132 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
jk41126 0:81b42d622dc5 133 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
jk41126 0:81b42d622dc5 134 pwm2.write(PIout_2 + 0.5f);
jk41126 0:81b42d622dc5 135 TIM1->CCER |= 0x40;
jk41126 0:81b42d622dc5 136
jk41126 0:81b42d622dc5 137
jk41126 0:81b42d622dc5 138 }
jk41126 0:81b42d622dc5 139
jk41126 0:81b42d622dc5 140 void CN_interrupt(void)
jk41126 0:81b42d622dc5 141 {
jk41126 0:81b42d622dc5 142 //Motor 1
jk41126 0:81b42d622dc5 143 stateA_1 = HallA_1.read();
jk41126 0:81b42d622dc5 144 stateB_1 = HallB_1.read();
jk41126 0:81b42d622dc5 145
jk41126 0:81b42d622dc5 146 ///code for state determination///
jk41126 0:81b42d622dc5 147
jk41126 0:81b42d622dc5 148 if(stateA_1 == stateB_1_old && c == 1)
jk41126 0:81b42d622dc5 149 {
jk41126 0:81b42d622dc5 150 v1Count = v1Count + 1;
jk41126 0:81b42d622dc5 151 }
jk41126 0:81b42d622dc5 152
jk41126 0:81b42d622dc5 153 if(stateB_1 == stateA_1_old && c == 1)
jk41126 0:81b42d622dc5 154 {
jk41126 0:81b42d622dc5 155 v1Count = v1Count - 1;
jk41126 0:81b42d622dc5 156 }
jk41126 0:81b42d622dc5 157
jk41126 0:81b42d622dc5 158 c = 1;
jk41126 0:81b42d622dc5 159 stateA_1_old = stateA_1;
jk41126 0:81b42d622dc5 160 stateB_1_old = stateB_1;
jk41126 0:81b42d622dc5 161
jk41126 0:81b42d622dc5 162 //////////////////////////////////
jk41126 0:81b42d622dc5 163
jk41126 0:81b42d622dc5 164
jk41126 0:81b42d622dc5 165 //Motor 2
jk41126 0:81b42d622dc5 166 stateA_2 = HallA_2.read();
jk41126 0:81b42d622dc5 167 stateB_2 = HallB_2.read();
jk41126 0:81b42d622dc5 168
jk41126 0:81b42d622dc5 169 ///code for state determination///
jk41126 0:81b42d622dc5 170
jk41126 0:81b42d622dc5 171 if(stateA_2 == stateB_2_old && d == 1)
jk41126 0:81b42d622dc5 172 {
jk41126 0:81b42d622dc5 173 v2Count = v2Count + 1;
jk41126 0:81b42d622dc5 174 }
jk41126 0:81b42d622dc5 175
jk41126 0:81b42d622dc5 176 if(stateB_2 == stateA_2_old && d == 1)
jk41126 0:81b42d622dc5 177 {
jk41126 0:81b42d622dc5 178 v2Count = v2Count - 1;
jk41126 0:81b42d622dc5 179 }
jk41126 0:81b42d622dc5 180
jk41126 0:81b42d622dc5 181 d = 1;
jk41126 0:81b42d622dc5 182 stateA_2_old = stateA_2;
jk41126 0:81b42d622dc5 183 stateB_2_old = stateB_2;
jk41126 0:81b42d622dc5 184
jk41126 0:81b42d622dc5 185 //////////////////////////////////
jk41126 0:81b42d622dc5 186
jk41126 0:81b42d622dc5 187 }
jk41126 0:81b42d622dc5 188
jk41126 0:81b42d622dc5 189 void init_TIMER(void)
jk41126 0:81b42d622dc5 190 {
jk41126 0:81b42d622dc5 191 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
jk41126 0:81b42d622dc5 192 }
jk41126 0:81b42d622dc5 193
jk41126 0:81b42d622dc5 194 void init_PWM(void)
jk41126 0:81b42d622dc5 195 {
jk41126 0:81b42d622dc5 196 pwm1.period_us(50);
jk41126 0:81b42d622dc5 197 pwm1.write(0.5);
jk41126 0:81b42d622dc5 198 TIM1->CCER |= 0x4;
jk41126 0:81b42d622dc5 199
jk41126 0:81b42d622dc5 200 pwm2.period_us(50);
jk41126 0:81b42d622dc5 201 pwm2.write(0.5);
jk41126 0:81b42d622dc5 202 TIM1->CCER |= 0x40;
jk41126 0:81b42d622dc5 203 }
jk41126 0:81b42d622dc5 204
jk41126 0:81b42d622dc5 205 void init_CN(void)
jk41126 0:81b42d622dc5 206 {
jk41126 0:81b42d622dc5 207 HallA_1.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 208 HallA_1.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 209 HallB_1.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 210 HallB_1.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 211
jk41126 0:81b42d622dc5 212 HallA_2.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 213 HallA_2.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 214 HallB_2.rise(&CN_interrupt);
jk41126 0:81b42d622dc5 215 HallB_2.fall(&CN_interrupt);
jk41126 0:81b42d622dc5 216
jk41126 0:81b42d622dc5 217 stateA_1 = HallA_1.read();
jk41126 0:81b42d622dc5 218 stateB_1 = HallB_1.read();
jk41126 0:81b42d622dc5 219 stateA_2 = HallA_2.read();
jk41126 0:81b42d622dc5 220 stateB_2 = HallB_2.read();
jk41126 0:81b42d622dc5 221 }
jk41126 0:81b42d622dc5 222
jk41126 0:81b42d622dc5 223 void BT_interrupt(void)
jk41126 0:81b42d622dc5 224 {
jk41126 0:81b42d622dc5 225 int Receive_Counter,Receive_Flag;
jk41126 0:81b42d622dc5 226
jk41126 0:81b42d622dc5 227 static char Temp;
jk41126 0:81b42d622dc5 228 Temp = bluetooth.getc();
jk41126 0:81b42d622dc5 229
jk41126 0:81b42d622dc5 230 if (Receive_Flag==1)
jk41126 0:81b42d622dc5 231 {
jk41126 0:81b42d622dc5 232 Receive_Counter++;
jk41126 0:81b42d622dc5 233 Receive_Data[Receive_Counter]=Temp;
jk41126 0:81b42d622dc5 234
jk41126 0:81b42d622dc5 235 if(Receive_Counter==9)
jk41126 0:81b42d622dc5 236 {
jk41126 0:81b42d622dc5 237 Command_Flag=1;
jk41126 0:81b42d622dc5 238 Receive_Flag=0;
jk41126 0:81b42d622dc5 239 Receive_Counter=0;
jk41126 0:81b42d622dc5 240 }
jk41126 0:81b42d622dc5 241 }
jk41126 0:81b42d622dc5 242 else
jk41126 0:81b42d622dc5 243 {
jk41126 0:81b42d622dc5 244 if(Temp==36)
jk41126 0:81b42d622dc5 245 {
jk41126 0:81b42d622dc5 246 Receive_Flag=1;
jk41126 0:81b42d622dc5 247 Receive_Counter=0;
jk41126 0:81b42d622dc5 248 Receive_Data[0]= Temp;
jk41126 0:81b42d622dc5 249 }
jk41126 0:81b42d622dc5 250 }
jk41126 0:81b42d622dc5 251 }