Robotic_winteam / Mbed 2 deprecated Nucleo_pwmalla

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

Fork of Nucleo_pwmalla by 皓翔 楊

main.cpp@1:c4f94a2d3cba, 2016-05-25 (annotated)

Committer:
ooseausername
Date:
Wed May 25 08:24:45 2016 +0000
Revision:
1:c4f94a2d3cba
Parent:
0:7a08321141df 
jbh

Who changed what in which revision?

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