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

AI_Friday.cpp@15:3e0a4079b28a, 2016-05-25 (annotated)

Committer:
roger5641
Date:
Wed May 25 11:08:07 2016 +0000
Revision:
15:3e0a4079b28a
Parent:
14:c46f22bcaa38
Child:
16:a102929b2228
BLE

Who changed what in which revision?

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