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@0:81b42d622dc5, 2016-05-26 (annotated)

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