Robotics Term Project / Mbed 2 deprecated Robottics_Motion

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@19:5091c934ebd0, 2016-05-25 (annotated)

Committer:
roger5641
Date:
Wed May 25 13:38:27 2016 +0000
Revision:
19:5091c934ebd0
Parent:
18:2db6c97a4145
Child:
26:dbdbfdb4dd41
AI;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger5641 0:2ac10e7b6b03 1 /*LAB_DCMotor*/
roger5641 0:2ac10e7b6b03 2 #include "mbed.h"
roger5641 0:2ac10e7b6b03 3
roger5641 0:2ac10e7b6b03 4 //The number will be compiled as type "double" in default
roger5641 0:2ac10e7b6b03 5 //Add a "f" after the number can make it compiled as type "float"
roger5641 0:2ac10e7b6b03 6 #define Ts 0.01f //period of timer1 (s)
roger5641 0:2ac10e7b6b03 7 #define Kp 0.0025f
roger5641 0:2ac10e7b6b03 8 #define Ki 0.008f
roger5641 0:2ac10e7b6b03 9
roger5641 0:2ac10e7b6b03 10 PwmOut pwm1(D7);
roger5641 0:2ac10e7b6b03 11 PwmOut pwm1n(D11);
roger5641 0:2ac10e7b6b03 12 PwmOut pwm2(D8);
roger5641 0:2ac10e7b6b03 13 PwmOut pwm2n(A3);
roger5641 0:2ac10e7b6b03 14 PwmOut servo(A0);
roger5641 0:2ac10e7b6b03 15
roger5641 0:2ac10e7b6b03 16 Serial bluetooth(D10,D2);
roger5641 0:2ac10e7b6b03 17 Serial pc(D1, D0);
roger5641 0:2ac10e7b6b03 18
roger5641 0:2ac10e7b6b03 19 DigitalOut led1(A4);
roger5641 0:2ac10e7b6b03 20 DigitalOut led2(A5);
roger5641 0:2ac10e7b6b03 21
roger5641 0:2ac10e7b6b03 22 //Motor1 sensor
roger5641 0:2ac10e7b6b03 23 InterruptIn HallA_1(A1);
roger5641 0:2ac10e7b6b03 24 InterruptIn HallB_1(A2);
roger5641 0:2ac10e7b6b03 25 //Motor2 sensor
roger5641 0:2ac10e7b6b03 26 InterruptIn HallA_2(D13);
roger5641 0:2ac10e7b6b03 27 InterruptIn HallB_2(D12);
roger5641 0:2ac10e7b6b03 28
roger5641 0:2ac10e7b6b03 29 Ticker timer1;
roger5641 0:2ac10e7b6b03 30 void timer1_interrupt(void);
roger5641 0:2ac10e7b6b03 31 void CN_interrupt(void);
roger5641 8:079c3326816e 32 void _ISR_U2RXInterrupt(void);
roger5641 0:2ac10e7b6b03 33
roger5641 0:2ac10e7b6b03 34 void init_TIMER(void);
roger5641 0:2ac10e7b6b03 35 void init_PWM(void);
roger5641 0:2ac10e7b6b03 36 void init_CN(void);
roger5641 0:2ac10e7b6b03 37 void init_err(void);
roger5641 0:2ac10e7b6b03 38
roger5641 0:2ac10e7b6b03 39 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
roger5641 0:2ac10e7b6b03 40 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
roger5641 0:2ac10e7b6b03 41
roger5641 0:2ac10e7b6b03 42 int v1Count = 0;
roger5641 0:2ac10e7b6b03 43 int v2Count = 0;
roger5641 0:2ac10e7b6b03 44
roger5641 0:2ac10e7b6b03 45 float v1 = 0.0, v1_ref = 0.0;
roger5641 0:2ac10e7b6b03 46 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
roger5641 0:2ac10e7b6b03 47 float v2 = 0.0, v2_ref = 0.0;
roger5641 0:2ac10e7b6b03 48 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
roger5641 0:2ac10e7b6b03 49
roger5641 0:2ac10e7b6b03 50 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
roger5641 0:2ac10e7b6b03 51
roger5641 19:5091c934ebd0 52 int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0;
roger5641 8:079c3326816e 53 int Receive_Data[33];
roger5641 8:079c3326816e 54
roger5641 8:079c3326816e 55 int Distance_Target = 0, Angle_Target = 0;
roger5641 8:079c3326816e 56 int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
roger5641 8:079c3326816e 57 int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
roger5641 8:079c3326816e 58 int pwm_duty;
roger5641 8:079c3326816e 59
roger5641 0:2ac10e7b6b03 60 int main() {
roger5641 0:2ac10e7b6b03 61
roger5641 0:2ac10e7b6b03 62 init_TIMER();
roger5641 0:2ac10e7b6b03 63 init_PWM();
roger5641 0:2ac10e7b6b03 64 init_CN();
roger5641 0:2ac10e7b6b03 65
roger5641 0:2ac10e7b6b03 66 bluetooth.baud(115200); //設定鮑率
roger5641 0:2ac10e7b6b03 67 pc.baud(57600);
roger5641 0:2ac10e7b6b03 68
roger5641 0:2ac10e7b6b03 69
roger5641 8:079c3326816e 70
roger5641 0:2ac10e7b6b03 71 while(1)
roger5641 0:2ac10e7b6b03 72 {
roger5641 0:2ac10e7b6b03 73 if(pc.readable())
roger5641 0:2ac10e7b6b03 74 {
roger5641 0:2ac10e7b6b03 75 bluetooth.putc(pc.getc());
roger5641 0:2ac10e7b6b03 76 }
roger5641 0:2ac10e7b6b03 77 if(bluetooth.readable())
roger5641 0:2ac10e7b6b03 78 {
roger5641 0:2ac10e7b6b03 79 pc.putc(bluetooth.getc());
roger5641 0:2ac10e7b6b03 80 }
roger5641 8:079c3326816e 81
roger5641 8:079c3326816e 82 if(Command_Flag == 1)
roger5641 8:079c3326816e 83 {
roger5641 8:079c3326816e 84 //read data from matlab
roger5641 8:079c3326816e 85 //distance_target
roger5641 8:079c3326816e 86 Distance_Target = (Receive_Data[2]-0x30)*100 + (Receive_Data[3]-0x30)*10 + (Receive_Data[4]-0x30);
roger5641 8:079c3326816e 87
roger5641 8:079c3326816e 88 if(Receive_Data[1] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 89 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 90
roger5641 8:079c3326816e 91 //ang_rel_target
roger5641 8:079c3326816e 92 Angle_Target = (Receive_Data[6]-0x30)*100 + (Receive_Data[7]-0x30)*10 + (Receive_Data[8]-0x30);
roger5641 8:079c3326816e 93
roger5641 8:079c3326816e 94 if(Receive_Data[5] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 95 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 96
roger5641 8:079c3326816e 97 //x_position_car_1
roger5641 8:079c3326816e 98 X_Position_1 = (Receive_Data[10]-0x30)*100 + (Receive_Data[11]-0x30)*10 + (Receive_Data[12]-0x30);
roger5641 8:079c3326816e 99
roger5641 8:079c3326816e 100 if(Receive_Data[9] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 101 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 102
roger5641 8:079c3326816e 103 //y_position_car_1
roger5641 8:079c3326816e 104 Y_Position_1 = (Receive_Data[14]-0x30)*100 + (Receive_Data[15]-0x30)*10 + (Receive_Data[16]-0x30);
roger5641 8:079c3326816e 105
roger5641 8:079c3326816e 106 if(Receive_Data[13] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 107 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 108
roger5641 8:079c3326816e 109 //angle_car_1
roger5641 8:079c3326816e 110 Angle_1 = (Receive_Data[18]-0x30)*100 + (Receive_Data[19]-0x30)*10 + (Receive_Data[20]-0x30);
roger5641 8:079c3326816e 111
roger5641 8:079c3326816e 112 if(Receive_Data[17] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 113 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 114
roger5641 8:079c3326816e 115 //x_position_car_2
roger5641 8:079c3326816e 116 X_Position_2 = (Receive_Data[22]-0x30)*100 + (Receive_Data[23]-0x30)*10 + (Receive_Data[24]-0x30);
roger5641 8:079c3326816e 117
roger5641 8:079c3326816e 118 if(Receive_Data[21] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 119 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 120
roger5641 8:079c3326816e 121 //y_position_car_2
roger5641 8:079c3326816e 122 Y_Position_2 = (Receive_Data[26]-0x30)*100 + (Receive_Data[27]-0x30)*10 + (Receive_Data[28]-0x30);
roger5641 8:079c3326816e 123
roger5641 8:079c3326816e 124 if(Receive_Data[25] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 125 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 126
roger5641 8:079c3326816e 127 //angle_car_1
roger5641 8:079c3326816e 128 Angle_2 = (Receive_Data[30]-0x30)*100 + (Receive_Data[31]-0x30)*10 + (Receive_Data[32]-0x30);
roger5641 8:079c3326816e 129
roger5641 8:079c3326816e 130 if(Receive_Data[29] == '-')Distance_Target = -1* Distance_Target;
roger5641 8:079c3326816e 131 else Distance_Target = Distance_Target;
roger5641 8:079c3326816e 132
roger5641 8:079c3326816e 133 // PWM
roger5641 8:079c3326816e 134 if(Receive_Data[33] == 'j')pwm_duty = 148;
roger5641 8:079c3326816e 135 else if(Receive_Data[33] == 'k')pwm_duty = 100;
roger5641 8:079c3326816e 136
roger5641 8:079c3326816e 137 Command_Flag = 0;
roger5641 8:079c3326816e 138 }
roger5641 8:079c3326816e 139
roger5641 8:079c3326816e 140
roger5641 0:2ac10e7b6b03 141 }
roger5641 0:2ac10e7b6b03 142 }
roger5641 0:2ac10e7b6b03 143
roger5641 0:2ac10e7b6b03 144 void timer1_interrupt(void)
roger5641 0:2ac10e7b6b03 145 {
roger5641 0:2ac10e7b6b03 146 //Motor 1
roger5641 0:2ac10e7b6b03 147 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
roger5641 0:2ac10e7b6b03 148 v1Count = 0;
roger5641 0:2ac10e7b6b03 149
roger5641 0:2ac10e7b6b03 150 ///code for PI control///
roger5641 0:2ac10e7b6b03 151 v1_err = v1_ref - v1;
roger5641 0:2ac10e7b6b03 152 v1_ierr = Ts*v1_err + v1_ierr;
roger5641 0:2ac10e7b6b03 153 PIout_1 = Kp*v1_err + Ki*v1_ierr;
roger5641 0:2ac10e7b6b03 154
roger5641 0:2ac10e7b6b03 155 /////////////////////////
roger5641 0:2ac10e7b6b03 156
roger5641 0:2ac10e7b6b03 157 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
roger5641 0:2ac10e7b6b03 158 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
roger5641 0:2ac10e7b6b03 159 pwm1.write(PIout_1 + 0.5f);
roger5641 0:2ac10e7b6b03 160 TIM1->CCER |= 0x4;
roger5641 0:2ac10e7b6b03 161
roger5641 0:2ac10e7b6b03 162
roger5641 0:2ac10e7b6b03 163 //Motor 2
roger5641 0:2ac10e7b6b03 164 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
roger5641 0:2ac10e7b6b03 165 v2Count = 0;
roger5641 0:2ac10e7b6b03 166
roger5641 0:2ac10e7b6b03 167 ///code for PI control///
roger5641 0:2ac10e7b6b03 168 v2_err = v2_ref - v2;
roger5641 0:2ac10e7b6b03 169 v2_ierr = Ts*v2_err + v2_ierr;
roger5641 0:2ac10e7b6b03 170 PIout_2 = Kp*v2_err + Ki*v2_ierr;
roger5641 0:2ac10e7b6b03 171
roger5641 0:2ac10e7b6b03 172 /////////////////////////
roger5641 0:2ac10e7b6b03 173
roger5641 0:2ac10e7b6b03 174 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
roger5641 0:2ac10e7b6b03 175 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
roger5641 0:2ac10e7b6b03 176 pwm2.write(PIout_2 + 0.5f);
roger5641 0:2ac10e7b6b03 177 TIM1->CCER |= 0x40;
roger5641 0:2ac10e7b6b03 178
roger5641 0:2ac10e7b6b03 179 switch(bluetooth.getc())
roger5641 0:2ac10e7b6b03 180 {
roger5641 0:2ac10e7b6b03 181 case '1':
roger5641 0:2ac10e7b6b03 182 v1_ref = 30;
roger5641 0:2ac10e7b6b03 183 v2_ref = 30;
roger5641 0:2ac10e7b6b03 184 init_err();
roger5641 0:2ac10e7b6b03 185 break;
roger5641 0:2ac10e7b6b03 186 case '2':
roger5641 0:2ac10e7b6b03 187 v1_ref = 40;
roger5641 0:2ac10e7b6b03 188 v2_ref = 40;
roger5641 0:2ac10e7b6b03 189 init_err();
roger5641 0:2ac10e7b6b03 190 break;
roger5641 0:2ac10e7b6b03 191 }
roger5641 0:2ac10e7b6b03 192
roger5641 0:2ac10e7b6b03 193 // Servo
roger5641 0:2ac10e7b6b03 194
roger5641 8:079c3326816e 195 //servo_duty = 0.079 + (0.084/180)*angle;
roger5641 0:2ac10e7b6b03 196 servo.write(servo_duty);
roger5641 0:2ac10e7b6b03 197 servo = 1;
roger5641 0:2ac10e7b6b03 198 wait(0.1);
roger5641 0:2ac10e7b6b03 199 servo = 0;
roger5641 0:2ac10e7b6b03 200
roger5641 0:2ac10e7b6b03 201 ////////////////////////////////////////////
roger5641 0:2ac10e7b6b03 202
roger5641 0:2ac10e7b6b03 203 if(servo_duty >= 0.121f)servo_duty = 0.121;
roger5641 0:2ac10e7b6b03 204 else if(servo_duty <= 0.037f)servo_duty = 0.037;
roger5641 0:2ac10e7b6b03 205 servo.write(servo_duty);
roger5641 0:2ac10e7b6b03 206
roger5641 0:2ac10e7b6b03 207 }
roger5641 0:2ac10e7b6b03 208
roger5641 0:2ac10e7b6b03 209 void CN_interrupt(void)
roger5641 0:2ac10e7b6b03 210 {
roger5641 0:2ac10e7b6b03 211 //Motor 1
roger5641 0:2ac10e7b6b03 212 stateA_1 = HallA_1.read();
roger5641 0:2ac10e7b6b03 213 stateB_1 = HallB_1.read();
roger5641 0:2ac10e7b6b03 214
roger5641 0:2ac10e7b6b03 215 ///code for state determination///
roger5641 0:2ac10e7b6b03 216 if(stateA_1==0&&stateB_1==0){
roger5641 0:2ac10e7b6b03 217 state_1 = 1;}
roger5641 0:2ac10e7b6b03 218 else if(stateA_1==0&&stateB_1==1){
roger5641 0:2ac10e7b6b03 219 state_1 = 2;}
roger5641 0:2ac10e7b6b03 220 else if(stateA_1==1&&stateB_1==1){
roger5641 0:2ac10e7b6b03 221 state_1 = 3;}
roger5641 0:2ac10e7b6b03 222 else if(stateA_1==1&&stateB_1==0){
roger5641 0:2ac10e7b6b03 223 state_1 = 4;}
roger5641 0:2ac10e7b6b03 224
roger5641 0:2ac10e7b6b03 225 if(state_1 == 1)
roger5641 0:2ac10e7b6b03 226 {
roger5641 0:2ac10e7b6b03 227 if(state_1-state_1_old == -3)
roger5641 0:2ac10e7b6b03 228 v1Count=v1Count+1;
roger5641 0:2ac10e7b6b03 229 else if(state_1-state_1_old == -1)
roger5641 0:2ac10e7b6b03 230 v1Count=v1Count-1;
roger5641 0:2ac10e7b6b03 231 }
roger5641 0:2ac10e7b6b03 232 else if(state_1 == 2)
roger5641 0:2ac10e7b6b03 233 {
roger5641 0:2ac10e7b6b03 234 if(state_1-state_1_old == 1)
roger5641 0:2ac10e7b6b03 235 v1Count=v1Count+1;
roger5641 0:2ac10e7b6b03 236 else if(state_1-state_1_old == -1)
roger5641 0:2ac10e7b6b03 237 v1Count=v1Count-1;
roger5641 0:2ac10e7b6b03 238 }
roger5641 0:2ac10e7b6b03 239 else if(state_1 == 3)
roger5641 0:2ac10e7b6b03 240 {
roger5641 0:2ac10e7b6b03 241 if(state_1-state_1_old == 1)
roger5641 0:2ac10e7b6b03 242 v1Count=v1Count+1;
roger5641 0:2ac10e7b6b03 243 else if(state_1-state_1_old == -1)
roger5641 0:2ac10e7b6b03 244 v1Count=v1Count-1;
roger5641 0:2ac10e7b6b03 245 }
roger5641 0:2ac10e7b6b03 246 else if(state_1 == 4)
roger5641 0:2ac10e7b6b03 247 {
roger5641 0:2ac10e7b6b03 248 if(state_1-state_1_old == 1)
roger5641 0:2ac10e7b6b03 249 v1Count=v1Count+1;
roger5641 0:2ac10e7b6b03 250 else if(state_1-state_1_old == 3)
roger5641 0:2ac10e7b6b03 251 v1Count=v1Count-1;
roger5641 0:2ac10e7b6b03 252 }
roger5641 0:2ac10e7b6b03 253 state_1_old = state_1;
roger5641 0:2ac10e7b6b03 254
roger5641 0:2ac10e7b6b03 255
roger5641 0:2ac10e7b6b03 256 //////////////////////////////////
roger5641 0:2ac10e7b6b03 257
roger5641 0:2ac10e7b6b03 258 //Forward
roger5641 0:2ac10e7b6b03 259 //v1Count +1
roger5641 0:2ac10e7b6b03 260 //Inverse
roger5641 0:2ac10e7b6b03 261 //v1Count -1
roger5641 0:2ac10e7b6b03 262
roger5641 0:2ac10e7b6b03 263
roger5641 0:2ac10e7b6b03 264 //Motor 2
roger5641 0:2ac10e7b6b03 265 stateA_2 = HallA_2.read();
roger5641 0:2ac10e7b6b03 266 stateB_2 = HallB_2.read();
roger5641 0:2ac10e7b6b03 267
roger5641 0:2ac10e7b6b03 268 ///code for state determination///
roger5641 0:2ac10e7b6b03 269 if(stateA_2==0&&stateB_2==0){
roger5641 0:2ac10e7b6b03 270 state_2 = 1;}
roger5641 0:2ac10e7b6b03 271 else if(stateA_2==0&&stateB_2==1){
roger5641 0:2ac10e7b6b03 272 state_2 = 2;}
roger5641 0:2ac10e7b6b03 273 else if(stateA_2==1&&stateB_2==1){
roger5641 0:2ac10e7b6b03 274 state_2 = 3;}
roger5641 0:2ac10e7b6b03 275 else if(stateA_2==1&&stateB_2==0){
roger5641 0:2ac10e7b6b03 276 state_2 = 4;}
roger5641 0:2ac10e7b6b03 277
roger5641 0:2ac10e7b6b03 278 if(state_2 == 1)
roger5641 0:2ac10e7b6b03 279 {
roger5641 0:2ac10e7b6b03 280 if(state_2-state_2_old == -3)
roger5641 0:2ac10e7b6b03 281 v2Count=v2Count+1;
roger5641 0:2ac10e7b6b03 282 else if(state_2-state_2_old == -1)
roger5641 0:2ac10e7b6b03 283 v2Count=v2Count-1;
roger5641 0:2ac10e7b6b03 284 }
roger5641 0:2ac10e7b6b03 285 else if(state_2 == 2)
roger5641 0:2ac10e7b6b03 286 {
roger5641 0:2ac10e7b6b03 287 if(state_2-state_2_old == 1)
roger5641 0:2ac10e7b6b03 288 v2Count=v2Count+1;
roger5641 0:2ac10e7b6b03 289 else if(state_2-state_2_old == -1)
roger5641 0:2ac10e7b6b03 290 v2Count=v2Count-1;
roger5641 0:2ac10e7b6b03 291 }
roger5641 0:2ac10e7b6b03 292 else if(state_2 == 3)
roger5641 0:2ac10e7b6b03 293 {
roger5641 0:2ac10e7b6b03 294 if(state_2-state_2_old == 1)
roger5641 0:2ac10e7b6b03 295 v2Count=v2Count+1;
roger5641 0:2ac10e7b6b03 296 else if(state_2-state_2_old == -1)
roger5641 0:2ac10e7b6b03 297 v2Count=v2Count-1;
roger5641 0:2ac10e7b6b03 298 }
roger5641 0:2ac10e7b6b03 299 else if(state_2 == 4)
roger5641 0:2ac10e7b6b03 300 {
roger5641 0:2ac10e7b6b03 301 if(state_2-state_2_old == 1)
roger5641 0:2ac10e7b6b03 302 v2Count=v2Count+1;
roger5641 0:2ac10e7b6b03 303 else if(state_2-state_2_old == 3)
roger5641 0:2ac10e7b6b03 304 v2Count=v2Count-1;
roger5641 0:2ac10e7b6b03 305 }
roger5641 0:2ac10e7b6b03 306 state_2_old = state_2;
roger5641 0:2ac10e7b6b03 307
roger5641 0:2ac10e7b6b03 308 //////////////////////////////////
roger5641 0:2ac10e7b6b03 309
roger5641 0:2ac10e7b6b03 310 //Forward
roger5641 0:2ac10e7b6b03 311 //v2Count +1
roger5641 0:2ac10e7b6b03 312 //Inverse
roger5641 0:2ac10e7b6b03 313 //v2Count -1
roger5641 0:2ac10e7b6b03 314 }
roger5641 0:2ac10e7b6b03 315
roger5641 8:079c3326816e 316 void _ISR_U2RXInterrupt(void)
roger5641 8:079c3326816e 317 {
roger5641 8:079c3326816e 318 /////////// Receive ////////////
roger5641 8:079c3326816e 319 static char Temp;
roger5641 8:079c3326816e 320 Temp = U2RXREG;
roger5641 8:079c3326816e 321
roger5641 8:079c3326816e 322 if(Receive_Flag == 1)
roger5641 8:079c3326816e 323 {
roger5641 8:079c3326816e 324 Receive_Counter++;
roger5641 8:079c3326816e 325 Receive_Data[Receive_Counter] = Temp;
roger5641 8:079c3326816e 326
roger5641 8:079c3326816e 327 if(Receive_Counter == 33) // 8 data *4byte
roger5641 8:079c3326816e 328 {
roger5641 8:079c3326816e 329 //Send_Flag == 1
roger5641 8:079c3326816e 330 Command_Flag = 1;
roger5641 8:079c3326816e 331 Receive_Flag = 0;
roger5641 8:079c3326816e 332 Receive_Counter = 0;
roger5641 8:079c3326816e 333 }
roger5641 8:079c3326816e 334 }
roger5641 8:079c3326816e 335
roger5641 8:079c3326816e 336 else
roger5641 8:079c3326816e 337 {
roger5641 8:079c3326816e 338 if(Temp == 36) //'$'
roger5641 8:079c3326816e 339 {
roger5641 8:079c3326816e 340 Receive_Flag = 1;
roger5641 8:079c3326816e 341 Receive_Counter = 0;
roger5641 8:079c3326816e 342 Receive_Data[0] = Temp;
roger5641 8:079c3326816e 343 }
roger5641 8:079c3326816e 344 else
roger5641 8:079c3326816e 345 {
roger5641 8:079c3326816e 346 // Waiting
roger5641 8:079c3326816e 347 }
roger5641 8:079c3326816e 348 }
roger5641 8:079c3326816e 349
roger5641 8:079c3326816e 350 IFS1bits.U2RXIF = 0;
roger5641 8:079c3326816e 351 }
roger5641 8:079c3326816e 352
roger5641 8:079c3326816e 353
roger5641 0:2ac10e7b6b03 354 void init_TIMER(void)
roger5641 0:2ac10e7b6b03 355 {
roger5641 0:2ac10e7b6b03 356 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
roger5641 0:2ac10e7b6b03 357 }
roger5641 0:2ac10e7b6b03 358
roger5641 0:2ac10e7b6b03 359 void init_PWM(void)
roger5641 0:2ac10e7b6b03 360 {
roger5641 0:2ac10e7b6b03 361 pwm1.period_us(50);
roger5641 0:2ac10e7b6b03 362 pwm1.write(0.5);
roger5641 0:2ac10e7b6b03 363 TIM1->CCER |= 0x4;
roger5641 0:2ac10e7b6b03 364
roger5641 0:2ac10e7b6b03 365 pwm2.period_us(50);
roger5641 0:2ac10e7b6b03 366 pwm2.write(0.5);
roger5641 0:2ac10e7b6b03 367 TIM1->CCER |= 0x40;
roger5641 0:2ac10e7b6b03 368 }
roger5641 0:2ac10e7b6b03 369
roger5641 0:2ac10e7b6b03 370 void init_CN(void)
roger5641 0:2ac10e7b6b03 371 {
roger5641 0:2ac10e7b6b03 372 HallA_1.rise(&CN_interrupt);
roger5641 0:2ac10e7b6b03 373 HallA_1.fall(&CN_interrupt);
roger5641 0:2ac10e7b6b03 374 HallB_1.rise(&CN_interrupt);
roger5641 0:2ac10e7b6b03 375 HallB_1.fall(&CN_interrupt);
roger5641 0:2ac10e7b6b03 376
roger5641 0:2ac10e7b6b03 377 HallA_2.rise(&CN_interrupt);
roger5641 0:2ac10e7b6b03 378 HallA_2.fall(&CN_interrupt);
roger5641 0:2ac10e7b6b03 379 HallB_2.rise(&CN_interrupt);
roger5641 0:2ac10e7b6b03 380 HallB_2.fall(&CN_interrupt);
roger5641 0:2ac10e7b6b03 381
roger5641 0:2ac10e7b6b03 382 stateA_1 = HallA_1.read();
roger5641 0:2ac10e7b6b03 383 stateB_1 = HallB_1.read();
roger5641 0:2ac10e7b6b03 384 stateA_2 = HallA_2.read();
roger5641 0:2ac10e7b6b03 385 stateB_2 = HallB_2.read();
roger5641 0:2ac10e7b6b03 386 }
roger5641 0:2ac10e7b6b03 387 void init_err(void)
roger5641 0:2ac10e7b6b03 388 {
roger5641 0:2ac10e7b6b03 389 v1_ierr = 0.0;
roger5641 0:2ac10e7b6b03 390 v2_ierr = 0.0;
roger5641 18:2db6c97a4145 391 }