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@0:7a08321141df, 2016-05-25 (annotated)

Committer:
ooseausername
Date:
Wed May 25 07:56:14 2016 +0000
Revision:
0:7a08321141df
Child:
1:c4f94a2d3cba
all code

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