Robotics Term Project
/
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@18:2db6c97a4145, 2016-05-25 (annotated)
- Committer:
- roger5641
- Date:
- Wed May 25 13:29:52 2016 +0000
- Revision:
- 18:2db6c97a4145
- Parent:
- 17:d96afd9d2692
- Child:
- 19:5091c934ebd0
AI motion
Who changed what in which revision?
| User | Revision | Line number | New 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 | |
| roger5641 | 17:d96afd9d2692 | 10 | |
| roger5641 | 17:d96afd9d2692 | 11 | PwmOut servo(A0); |
| smilestone520 | 10:06cc2b1d2aaf | 12 | PwmOut pwm1(D7); |
| smilestone520 | 10:06cc2b1d2aaf | 13 | PwmOut pwm1n(D11); |
| smilestone520 | 10:06cc2b1d2aaf | 14 | PwmOut pwm2(D8); |
| smilestone520 | 10:06cc2b1d2aaf | 15 | PwmOut pwm2n(A3); |
| smilestone520 | 10:06cc2b1d2aaf | 16 | |
| smilestone520 | 10:06cc2b1d2aaf | 17 | Serial bluetooth(D10,D2); |
| smilestone520 | 10:06cc2b1d2aaf | 18 | Serial pc(D1, D0); |
| smilestone520 | 10:06cc2b1d2aaf | 19 | |
| smilestone520 | 10:06cc2b1d2aaf | 20 | DigitalOut led1(A4); |
| smilestone520 | 10:06cc2b1d2aaf | 21 | DigitalOut led2(A5); |
| smilestone520 | 10:06cc2b1d2aaf | 22 | |
| smilestone520 | 10:06cc2b1d2aaf | 23 | //Motor1 sensor |
| smilestone520 | 10:06cc2b1d2aaf | 24 | InterruptIn HallA_1(A1); |
| smilestone520 | 10:06cc2b1d2aaf | 25 | InterruptIn HallB_1(A2); |
| smilestone520 | 10:06cc2b1d2aaf | 26 | //Motor2 sensor |
| smilestone520 | 10:06cc2b1d2aaf | 27 | InterruptIn HallA_2(D13); |
| smilestone520 | 10:06cc2b1d2aaf | 28 | InterruptIn HallB_2(D12); |
| smilestone520 | 10:06cc2b1d2aaf | 29 | |
| smilestone520 | 10:06cc2b1d2aaf | 30 | Ticker timer1; |
| smilestone520 | 10:06cc2b1d2aaf | 31 | void timer1_interrupt(void); |
| smilestone520 | 10:06cc2b1d2aaf | 32 | void CN_interrupt(void); |
| roger5641 | 17:d96afd9d2692 | 33 | //void _ISR_U2RXInterrupt(void); |
| smilestone520 | 10:06cc2b1d2aaf | 34 | |
| smilestone520 | 10:06cc2b1d2aaf | 35 | void init_TIMER(void); |
| smilestone520 | 10:06cc2b1d2aaf | 36 | void init_PWM(void); |
| smilestone520 | 10:06cc2b1d2aaf | 37 | void init_CN(void); |
| smilestone520 | 10:06cc2b1d2aaf | 38 | |
| smilestone520 | 10:06cc2b1d2aaf | 39 | int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0; |
| smilestone520 | 10:06cc2b1d2aaf | 40 | int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 41 | |
| smilestone520 | 10:06cc2b1d2aaf | 42 | int v1Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 43 | int v2Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 44 | |
| roger5641 | 17:d96afd9d2692 | 45 | float v1 = 0.0, v1_ref = 0.0; |
| smilestone520 | 10:06cc2b1d2aaf | 46 | float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0; |
| roger5641 | 17:d96afd9d2692 | 47 | float v2 = 0.0, v2_ref = 0.0; |
| smilestone520 | 10:06cc2b1d2aaf | 48 | float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0; |
| roger5641 | 17:d96afd9d2692 | 49 | |
| roger5641 | 17:d96afd9d2692 | 50 | int angle = 90; |
| roger5641 | 15:3e0a4079b28a | 51 | float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90 |
| roger5641 | 15:3e0a4079b28a | 52 | |
| roger5641 | 18:2db6c97a4145 | 53 | int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0; |
| roger5641 | 18:2db6c97a4145 | 54 | int Receive_Data[33] = {0}; |
| roger5641 | 15:3e0a4079b28a | 55 | |
| roger5641 | 18:2db6c97a4145 | 56 | double Distance_Target = 0, Angle_Target = 0; |
| roger5641 | 15:3e0a4079b28a | 57 | int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0; |
| roger5641 | 15:3e0a4079b28a | 58 | int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0; |
| roger5641 | 15:3e0a4079b28a | 59 | int pwm_duty; |
| roger5641 | 15:3e0a4079b28a | 60 | |
| smilestone520 | 11:44989c0bcea5 | 61 | //**** receive and return by bluetooth ************** // bluetooth.getc() |
| smilestone520 | 14:c46f22bcaa38 | 62 | float xC, yC; // car's position |
| smilestone520 | 14:c46f22bcaa38 | 63 | float angleC;// car's angle |
| roger5641 | 17:d96afd9d2692 | 64 | double xP,yP,distance; //position that car need to reach |
| smilestone520 | 14:c46f22bcaa38 | 65 | float angleR; // angleR = car-nextSpot direction ---> car direction |
| roger5641 | 17:d96afd9d2692 | 66 | int xGate = 800, yGate = 300; |
| roger5641 | 17:d96afd9d2692 | 67 | double angleGate, distanceGate; |
| smilestone520 | 11:44989c0bcea5 | 68 | int aI_State = 0; |
| roger5641 | 17:d96afd9d2692 | 69 | double pi = 3.1415926; |
| smilestone520 | 11:44989c0bcea5 | 70 | |
| smilestone520 | 10:06cc2b1d2aaf | 71 | int main() { |
| smilestone520 | 10:06cc2b1d2aaf | 72 | |
| smilestone520 | 10:06cc2b1d2aaf | 73 | init_TIMER(); |
| smilestone520 | 10:06cc2b1d2aaf | 74 | init_PWM(); |
| smilestone520 | 10:06cc2b1d2aaf | 75 | init_CN(); |
| roger5641 | 17:d96afd9d2692 | 76 | servo.write(0.079 + (0.084/180)*angle); |
| smilestone520 | 10:06cc2b1d2aaf | 77 | |
| smilestone520 | 10:06cc2b1d2aaf | 78 | bluetooth.baud(115200); //設定鮑率 |
| smilestone520 | 10:06cc2b1d2aaf | 79 | pc.baud(57600); |
| smilestone520 | 10:06cc2b1d2aaf | 80 | |
| smilestone520 | 10:06cc2b1d2aaf | 81 | |
| smilestone520 | 10:06cc2b1d2aaf | 82 | while(1) |
| smilestone520 | 10:06cc2b1d2aaf | 83 | { |
| smilestone520 | 10:06cc2b1d2aaf | 84 | if(pc.readable()) |
| smilestone520 | 10:06cc2b1d2aaf | 85 | { |
| smilestone520 | 10:06cc2b1d2aaf | 86 | bluetooth.putc(pc.getc()); |
| smilestone520 | 10:06cc2b1d2aaf | 87 | } |
| smilestone520 | 10:06cc2b1d2aaf | 88 | if(bluetooth.readable()) |
| smilestone520 | 10:06cc2b1d2aaf | 89 | { |
| smilestone520 | 10:06cc2b1d2aaf | 90 | pc.putc(bluetooth.getc()); |
| smilestone520 | 10:06cc2b1d2aaf | 91 | } |
| roger5641 | 15:3e0a4079b28a | 92 | |
| roger5641 | 17:d96afd9d2692 | 93 | /*if(Command_Flag == 1) |
| roger5641 | 15:3e0a4079b28a | 94 | { |
| roger5641 | 15:3e0a4079b28a | 95 | //read data from matlab |
| roger5641 | 15:3e0a4079b28a | 96 | //distance_target |
| roger5641 | 15:3e0a4079b28a | 97 | Distance_Target = (Receive_Data[2]-0x30)*100 + (Receive_Data[3]-0x30)*10 + (Receive_Data[4]-0x30); |
| roger5641 | 15:3e0a4079b28a | 98 | |
| roger5641 | 15:3e0a4079b28a | 99 | if(Receive_Data[1] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 100 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 101 | |
| roger5641 | 15:3e0a4079b28a | 102 | //ang_rel_target |
| roger5641 | 15:3e0a4079b28a | 103 | Angle_Target = (Receive_Data[6]-0x30)*100 + (Receive_Data[7]-0x30)*10 + (Receive_Data[8]-0x30); |
| roger5641 | 15:3e0a4079b28a | 104 | |
| roger5641 | 15:3e0a4079b28a | 105 | if(Receive_Data[5] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 106 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 107 | |
| roger5641 | 15:3e0a4079b28a | 108 | //x_position_car_1 |
| roger5641 | 15:3e0a4079b28a | 109 | X_Position_1 = (Receive_Data[10]-0x30)*100 + (Receive_Data[11]-0x30)*10 + (Receive_Data[12]-0x30); |
| roger5641 | 15:3e0a4079b28a | 110 | |
| roger5641 | 15:3e0a4079b28a | 111 | if(Receive_Data[9] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 112 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 113 | |
| roger5641 | 15:3e0a4079b28a | 114 | //y_position_car_1 |
| roger5641 | 15:3e0a4079b28a | 115 | Y_Position_1 = (Receive_Data[14]-0x30)*100 + (Receive_Data[15]-0x30)*10 + (Receive_Data[16]-0x30); |
| roger5641 | 15:3e0a4079b28a | 116 | |
| roger5641 | 15:3e0a4079b28a | 117 | if(Receive_Data[13] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 118 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 119 | |
| roger5641 | 15:3e0a4079b28a | 120 | //angle_car_1 |
| roger5641 | 15:3e0a4079b28a | 121 | Angle_1 = (Receive_Data[18]-0x30)*100 + (Receive_Data[19]-0x30)*10 + (Receive_Data[20]-0x30); |
| roger5641 | 15:3e0a4079b28a | 122 | |
| roger5641 | 15:3e0a4079b28a | 123 | if(Receive_Data[17] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 124 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 125 | |
| roger5641 | 15:3e0a4079b28a | 126 | //x_position_car_2 |
| roger5641 | 15:3e0a4079b28a | 127 | X_Position_2 = (Receive_Data[22]-0x30)*100 + (Receive_Data[23]-0x30)*10 + (Receive_Data[24]-0x30); |
| roger5641 | 15:3e0a4079b28a | 128 | |
| roger5641 | 15:3e0a4079b28a | 129 | if(Receive_Data[21] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 130 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 131 | |
| roger5641 | 15:3e0a4079b28a | 132 | //y_position_car_2 |
| roger5641 | 15:3e0a4079b28a | 133 | Y_Position_2 = (Receive_Data[26]-0x30)*100 + (Receive_Data[27]-0x30)*10 + (Receive_Data[28]-0x30); |
| roger5641 | 15:3e0a4079b28a | 134 | |
| roger5641 | 15:3e0a4079b28a | 135 | if(Receive_Data[25] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 136 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 137 | |
| roger5641 | 15:3e0a4079b28a | 138 | //angle_car_1 |
| roger5641 | 15:3e0a4079b28a | 139 | Angle_2 = (Receive_Data[30]-0x30)*100 + (Receive_Data[31]-0x30)*10 + (Receive_Data[32]-0x30); |
| roger5641 | 15:3e0a4079b28a | 140 | |
| roger5641 | 15:3e0a4079b28a | 141 | if(Receive_Data[29] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 142 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 143 | |
| roger5641 | 15:3e0a4079b28a | 144 | // PWM |
| roger5641 | 15:3e0a4079b28a | 145 | if(Receive_Data[33] == 'j')pwm_duty = 148; |
| roger5641 | 15:3e0a4079b28a | 146 | else if(Receive_Data[33] == 'k')pwm_duty = 100; |
| roger5641 | 15:3e0a4079b28a | 147 | |
| roger5641 | 15:3e0a4079b28a | 148 | Command_Flag = 0; |
| roger5641 | 17:d96afd9d2692 | 149 | }*/ |
| smilestone520 | 10:06cc2b1d2aaf | 150 | } |
| smilestone520 | 10:06cc2b1d2aaf | 151 | } |
| smilestone520 | 10:06cc2b1d2aaf | 152 | |
| smilestone520 | 10:06cc2b1d2aaf | 153 | void timer1_interrupt(void) |
| smilestone520 | 10:06cc2b1d2aaf | 154 | { |
| smilestone520 | 10:06cc2b1d2aaf | 155 | //Motor 1 |
| smilestone520 | 10:06cc2b1d2aaf | 156 | v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
| smilestone520 | 10:06cc2b1d2aaf | 157 | v1Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 158 | |
| smilestone520 | 10:06cc2b1d2aaf | 159 | ///code for PI control/// |
| smilestone520 | 10:06cc2b1d2aaf | 160 | v1_err = v1_ref - v1; |
| smilestone520 | 10:06cc2b1d2aaf | 161 | v1_ierr = 0.01f*v1_err + v1_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 162 | PIout_1 = Kp*v1_err + Ki*v1_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 163 | |
| smilestone520 | 10:06cc2b1d2aaf | 164 | if(PIout_1 >= 0.5f)PIout_1 = 0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 165 | else if(PIout_1 <= -0.5f)PIout_1 = -0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 166 | pwm1.write(PIout_1 + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 167 | TIM1->CCER |= 0x4; |
| 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 | if(PIout_2 >= 0.5f)PIout_2 = 0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 179 | else if(PIout_2 <= -0.5f)PIout_2 = -0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 180 | pwm2.write(PIout_2 + 0.5f); |
| smilestone520 | 10:06cc2b1d2aaf | 181 | TIM1->CCER |= 0x40; |
| smilestone520 | 10:06cc2b1d2aaf | 182 | |
| roger5641 | 17:d96afd9d2692 | 183 | |
| smilestone520 | 11:44989c0bcea5 | 184 | //***** main AI ************************** |
| smilestone520 | 11:44989c0bcea5 | 185 | |
| smilestone520 | 11:44989c0bcea5 | 186 | |
| smilestone520 | 11:44989c0bcea5 | 187 | //***** get position information from the bluetooth |
| roger5641 | 17:d96afd9d2692 | 188 | xC = X_Position_1; |
| roger5641 | 17:d96afd9d2692 | 189 | yC = Y_Position_1; // car's position |
| smilestone520 | 11:44989c0bcea5 | 190 | |
| roger5641 | 17:d96afd9d2692 | 191 | angleC = Angle_1; // car's angle |
| roger5641 | 17:d96afd9d2692 | 192 | |
| roger5641 | 17:d96afd9d2692 | 193 | distance = Distance_Target; |
| roger5641 | 18:2db6c97a4145 | 194 | xP = Distance_Target*cos(Angle_Target*pi/180); |
| roger5641 | 18:2db6c97a4145 | 195 | yP = Distance_Target*sin(Angle_Target*pi/180);//position that car need to reach |
| smilestone520 | 11:44989c0bcea5 | 196 | |
| roger5641 | 17:d96afd9d2692 | 197 | angleR = Angle_Target; // angleR = car-nextSpot direction ---> car direction |
| smilestone520 | 11:44989c0bcea5 | 198 | |
| smilestone520 | 11:44989c0bcea5 | 199 | //**** AI_State ******************** |
| smilestone520 | 11:44989c0bcea5 | 200 | |
| smilestone520 | 11:44989c0bcea5 | 201 | switch(aI_State) |
| smilestone520 | 11:44989c0bcea5 | 202 | { |
| smilestone520 | 12:c1a667ca6c53 | 203 | case 0: // IDLE |
| smilestone520 | 12:c1a667ca6c53 | 204 | // IDLE check if stop color appear |
| smilestone520 | 12:c1a667ca6c53 | 205 | //check purple color appear or not |
| roger5641 | 17:d96afd9d2692 | 206 | if(xP==0 && yP==0 && angleR==0) |
| roger5641 | 17:d96afd9d2692 | 207 | { |
| roger5641 | 17:d96afd9d2692 | 208 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 209 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 210 | } |
| roger5641 | 17:d96afd9d2692 | 211 | else |
| roger5641 | 17:d96afd9d2692 | 212 | aI_State = 1; |
| smilestone520 | 14:c46f22bcaa38 | 213 | //****setSpecs(); |
| smilestone520 | 11:44989c0bcea5 | 214 | break; |
| smilestone520 | 16:a102929b2228 | 215 | |
| roger5641 | 17:d96afd9d2692 | 216 | case 1: /// border condition |
| smilestone520 | 12:c1a667ca6c53 | 217 | // check if car fit border conditions |
| smilestone520 | 14:c46f22bcaa38 | 218 | //****funcBorder(); |
| roger5641 | 17:d96afd9d2692 | 219 | //****to case2 |
| smilestone520 | 11:44989c0bcea5 | 220 | break; |
| roger5641 | 17:d96afd9d2692 | 221 | case 2://move to get ball |
| roger5641 | 17:d96afd9d2692 | 222 | if(-3<=angleR<=3) // direct to the ball |
| roger5641 | 17:d96afd9d2692 | 223 | { |
| roger5641 | 17:d96afd9d2692 | 224 | if(distance<=10) // check夾具距離 |
| roger5641 | 17:d96afd9d2692 | 225 | { |
| roger5641 | 17:d96afd9d2692 | 226 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 227 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 228 | aI_State = 3; |
| roger5641 | 17:d96afd9d2692 | 229 | } |
| roger5641 | 17:d96afd9d2692 | 230 | else if(10<distance<50) |
| roger5641 | 17:d96afd9d2692 | 231 | { |
| roger5641 | 17:d96afd9d2692 | 232 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 233 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 234 | } |
| roger5641 | 17:d96afd9d2692 | 235 | else |
| roger5641 | 17:d96afd9d2692 | 236 | { |
| roger5641 | 17:d96afd9d2692 | 237 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 238 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 239 | } |
| roger5641 | 17:d96afd9d2692 | 240 | } |
| roger5641 | 17:d96afd9d2692 | 241 | |
| roger5641 | 17:d96afd9d2692 | 242 | else if(3<angleR<=15) // small angle right |
| roger5641 | 17:d96afd9d2692 | 243 | { |
| roger5641 | 17:d96afd9d2692 | 244 | if(distance<50) |
| roger5641 | 17:d96afd9d2692 | 245 | { |
| roger5641 | 17:d96afd9d2692 | 246 | v1_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 247 | v2_ref = -(v1_ref-5); |
| roger5641 | 17:d96afd9d2692 | 248 | } |
| roger5641 | 17:d96afd9d2692 | 249 | else |
| roger5641 | 17:d96afd9d2692 | 250 | { |
| roger5641 | 17:d96afd9d2692 | 251 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 252 | v2_ref = -(v1_ref-5); |
| roger5641 | 17:d96afd9d2692 | 253 | } |
| roger5641 | 17:d96afd9d2692 | 254 | |
| roger5641 | 17:d96afd9d2692 | 255 | } |
| roger5641 | 17:d96afd9d2692 | 256 | else if(-15<=angleR<-3) // small angle left |
| roger5641 | 17:d96afd9d2692 | 257 | { |
| roger5641 | 17:d96afd9d2692 | 258 | if(distance<50) |
| roger5641 | 17:d96afd9d2692 | 259 | { |
| roger5641 | 17:d96afd9d2692 | 260 | v1_ref = -(v2_ref-5); |
| roger5641 | 17:d96afd9d2692 | 261 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 262 | } |
| roger5641 | 17:d96afd9d2692 | 263 | else |
| roger5641 | 17:d96afd9d2692 | 264 | { |
| roger5641 | 17:d96afd9d2692 | 265 | v1_ref = -(v2_ref-5); |
| roger5641 | 17:d96afd9d2692 | 266 | v2_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 267 | } |
| roger5641 | 17:d96afd9d2692 | 268 | |
| roger5641 | 17:d96afd9d2692 | 269 | } |
| roger5641 | 17:d96afd9d2692 | 270 | else if(15<angleR<=180) // big angle right |
| roger5641 | 17:d96afd9d2692 | 271 | { |
| roger5641 | 17:d96afd9d2692 | 272 | v1_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 273 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 274 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 275 | if(distance<50) |
| roger5641 | 17:d96afd9d2692 | 276 | { |
| roger5641 | 17:d96afd9d2692 | 277 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 278 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 279 | } |
| roger5641 | 17:d96afd9d2692 | 280 | else |
| roger5641 | 17:d96afd9d2692 | 281 | { |
| roger5641 | 17:d96afd9d2692 | 282 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 283 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 284 | } |
| roger5641 | 17:d96afd9d2692 | 285 | |
| roger5641 | 17:d96afd9d2692 | 286 | } |
| roger5641 | 17:d96afd9d2692 | 287 | else if(-180<=angleR<-15) // big angle left |
| roger5641 | 17:d96afd9d2692 | 288 | { |
| roger5641 | 17:d96afd9d2692 | 289 | v1_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 290 | v2_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 291 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 292 | if(0<distance<50) |
| roger5641 | 17:d96afd9d2692 | 293 | { |
| roger5641 | 17:d96afd9d2692 | 294 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 295 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 296 | } |
| roger5641 | 17:d96afd9d2692 | 297 | else |
| roger5641 | 17:d96afd9d2692 | 298 | { |
| roger5641 | 17:d96afd9d2692 | 299 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 300 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 301 | } |
| roger5641 | 17:d96afd9d2692 | 302 | |
| roger5641 | 17:d96afd9d2692 | 303 | } |
| roger5641 | 17:d96afd9d2692 | 304 | |
| roger5641 | 17:d96afd9d2692 | 305 | break; |
| roger5641 | 17:d96afd9d2692 | 306 | case 3: //****getBall(); |
| roger5641 | 17:d96afd9d2692 | 307 | if((distance == 10) && (-3<=angleR<=3)) |
| roger5641 | 17:d96afd9d2692 | 308 | { |
| roger5641 | 17:d96afd9d2692 | 309 | angle = -10; |
| roger5641 | 17:d96afd9d2692 | 310 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 311 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 312 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 313 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 314 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 315 | } |
| roger5641 | 17:d96afd9d2692 | 316 | else |
| roger5641 | 17:d96afd9d2692 | 317 | aI_State = 2; |
| roger5641 | 17:d96afd9d2692 | 318 | |
| smilestone520 | 11:44989c0bcea5 | 319 | break; |
| smilestone520 | 12:c1a667ca6c53 | 320 | case 4: /// move to the gate and release ball |
| smilestone520 | 12:c1a667ca6c53 | 321 | // move to the point in front of the gate first |
| smilestone520 | 12:c1a667ca6c53 | 322 | // then head to the gate |
| roger5641 | 18:2db6c97a4145 | 323 | distanceGate = sqrt(((xGate-xP)*(xGate-xP))+((yGate-yP)*(yGate-yP))); |
| roger5641 | 17:d96afd9d2692 | 324 | angleGate = atan2((yGate-yP),(xGate-xP))*180/pi; |
| roger5641 | 17:d96afd9d2692 | 325 | if(-3<=angleGate<=3) // direct to the gate |
| roger5641 | 17:d96afd9d2692 | 326 | { |
| roger5641 | 17:d96afd9d2692 | 327 | if(distanceGate<=10) // check distance between car cemtroid and gate |
| roger5641 | 17:d96afd9d2692 | 328 | { |
| roger5641 | 17:d96afd9d2692 | 329 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 330 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 331 | aI_State = 5; |
| roger5641 | 17:d96afd9d2692 | 332 | } |
| roger5641 | 17:d96afd9d2692 | 333 | else if(10<distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 334 | { |
| roger5641 | 17:d96afd9d2692 | 335 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 336 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 337 | } |
| roger5641 | 17:d96afd9d2692 | 338 | else |
| roger5641 | 17:d96afd9d2692 | 339 | { |
| roger5641 | 17:d96afd9d2692 | 340 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 341 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 342 | } |
| roger5641 | 17:d96afd9d2692 | 343 | } |
| roger5641 | 17:d96afd9d2692 | 344 | else if(3<angleGate<=15) // small angle right |
| roger5641 | 17:d96afd9d2692 | 345 | { |
| roger5641 | 17:d96afd9d2692 | 346 | if(distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 347 | { |
| roger5641 | 17:d96afd9d2692 | 348 | v1_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 349 | v2_ref = -(v1_ref-5); |
| roger5641 | 17:d96afd9d2692 | 350 | } |
| roger5641 | 17:d96afd9d2692 | 351 | else |
| roger5641 | 17:d96afd9d2692 | 352 | { |
| roger5641 | 17:d96afd9d2692 | 353 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 354 | v2_ref = -(v1_ref-5); |
| roger5641 | 17:d96afd9d2692 | 355 | } |
| roger5641 | 17:d96afd9d2692 | 356 | } |
| roger5641 | 17:d96afd9d2692 | 357 | else if(-15<=angleGate<-3) // small angle left |
| roger5641 | 17:d96afd9d2692 | 358 | { |
| roger5641 | 17:d96afd9d2692 | 359 | if(distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 360 | { |
| roger5641 | 17:d96afd9d2692 | 361 | v1_ref = -(v2_ref-5); |
| roger5641 | 17:d96afd9d2692 | 362 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 363 | } |
| roger5641 | 17:d96afd9d2692 | 364 | else |
| roger5641 | 17:d96afd9d2692 | 365 | { |
| roger5641 | 17:d96afd9d2692 | 366 | v1_ref = -(v2_ref-5); |
| roger5641 | 17:d96afd9d2692 | 367 | v2_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 368 | } |
| roger5641 | 17:d96afd9d2692 | 369 | } |
| roger5641 | 17:d96afd9d2692 | 370 | else if(15<angleGate<=180) // big angle right |
| roger5641 | 17:d96afd9d2692 | 371 | { |
| roger5641 | 17:d96afd9d2692 | 372 | v1_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 373 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 374 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 375 | if(distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 376 | { |
| roger5641 | 17:d96afd9d2692 | 377 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 378 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 379 | } |
| roger5641 | 17:d96afd9d2692 | 380 | else |
| roger5641 | 17:d96afd9d2692 | 381 | { |
| roger5641 | 17:d96afd9d2692 | 382 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 383 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 384 | } |
| roger5641 | 17:d96afd9d2692 | 385 | } |
| roger5641 | 17:d96afd9d2692 | 386 | else if(-180<=angleGate<-15) // big angle left |
| roger5641 | 17:d96afd9d2692 | 387 | { |
| roger5641 | 17:d96afd9d2692 | 388 | v1_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 389 | v2_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 390 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 391 | if(0<distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 392 | { |
| roger5641 | 17:d96afd9d2692 | 393 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 394 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 395 | } |
| roger5641 | 17:d96afd9d2692 | 396 | else |
| roger5641 | 17:d96afd9d2692 | 397 | { |
| roger5641 | 17:d96afd9d2692 | 398 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 399 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 400 | } |
| roger5641 | 17:d96afd9d2692 | 401 | } |
| roger5641 | 17:d96afd9d2692 | 402 | |
| smilestone520 | 12:c1a667ca6c53 | 403 | break; |
| roger5641 | 17:d96afd9d2692 | 404 | case 5: // release ball |
| roger5641 | 17:d96afd9d2692 | 405 | if((-3<=angleGate<=3) && (distanceGate<=10)) // direct to the gate |
| roger5641 | 17:d96afd9d2692 | 406 | { |
| roger5641 | 17:d96afd9d2692 | 407 | angle = 90; // Fixture up |
| roger5641 | 17:d96afd9d2692 | 408 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 409 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 410 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 411 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 412 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 413 | |
| roger5641 | 17:d96afd9d2692 | 414 | pwm1.write(0.1f + 0.5f); // push |
| roger5641 | 17:d96afd9d2692 | 415 | pwm2.write(-0.1f + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 416 | wait(0.5); |
| roger5641 | 17:d96afd9d2692 | 417 | |
| roger5641 | 17:d96afd9d2692 | 418 | angle = -10; // Fixture down |
| roger5641 | 17:d96afd9d2692 | 419 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 420 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 421 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 422 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 423 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 424 | |
| roger5641 | 17:d96afd9d2692 | 425 | pwm1.write(-0.3f + 0.5f); // back and leave gate |
| roger5641 | 17:d96afd9d2692 | 426 | pwm2.write(0.3f + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 427 | wait(1); |
| roger5641 | 17:d96afd9d2692 | 428 | |
| roger5641 | 18:2db6c97a4145 | 429 | aI_State = 0; |
| roger5641 | 17:d96afd9d2692 | 430 | |
| roger5641 | 17:d96afd9d2692 | 431 | } |
| roger5641 | 17:d96afd9d2692 | 432 | |
| roger5641 | 17:d96afd9d2692 | 433 | break; |
| roger5641 | 17:d96afd9d2692 | 434 | |
| smilestone520 | 11:44989c0bcea5 | 435 | |
| smilestone520 | 11:44989c0bcea5 | 436 | } |
| smilestone520 | 11:44989c0bcea5 | 437 | |
| smilestone520 | 11:44989c0bcea5 | 438 | |
| smilestone520 | 10:06cc2b1d2aaf | 439 | } |
| smilestone520 | 10:06cc2b1d2aaf | 440 | |
| smilestone520 | 10:06cc2b1d2aaf | 441 | void CN_interrupt(void) |
| smilestone520 | 10:06cc2b1d2aaf | 442 | { |
| smilestone520 | 10:06cc2b1d2aaf | 443 | //Motor 1 |
| smilestone520 | 10:06cc2b1d2aaf | 444 | stateA_1 = HallA_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 445 | stateB_1 = HallB_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 446 | |
| smilestone520 | 10:06cc2b1d2aaf | 447 | ///code for state determination/// |
| smilestone520 | 10:06cc2b1d2aaf | 448 | if(stateA_1==0&&stateB_1==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 449 | state_1 = 1;} |
| smilestone520 | 10:06cc2b1d2aaf | 450 | else if(stateA_1==0&&stateB_1==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 451 | state_1 = 2;} |
| smilestone520 | 10:06cc2b1d2aaf | 452 | else if(stateA_1==1&&stateB_1==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 453 | state_1 = 3;} |
| smilestone520 | 10:06cc2b1d2aaf | 454 | else if(stateA_1==1&&stateB_1==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 455 | state_1 = 4;} |
| smilestone520 | 10:06cc2b1d2aaf | 456 | |
| smilestone520 | 10:06cc2b1d2aaf | 457 | if(state_1 == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 458 | { |
| smilestone520 | 10:06cc2b1d2aaf | 459 | if(state_1-state_1_old == -3) |
| smilestone520 | 10:06cc2b1d2aaf | 460 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 461 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 462 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 463 | } |
| smilestone520 | 10:06cc2b1d2aaf | 464 | else if(state_1 == 2) |
| smilestone520 | 10:06cc2b1d2aaf | 465 | { |
| smilestone520 | 10:06cc2b1d2aaf | 466 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 467 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 468 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 469 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 470 | } |
| smilestone520 | 10:06cc2b1d2aaf | 471 | else if(state_1 == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 472 | { |
| smilestone520 | 10:06cc2b1d2aaf | 473 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 474 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 475 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 476 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 477 | } |
| smilestone520 | 10:06cc2b1d2aaf | 478 | else if(state_1 == 4) |
| smilestone520 | 10:06cc2b1d2aaf | 479 | { |
| smilestone520 | 10:06cc2b1d2aaf | 480 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 481 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 482 | else if(state_1-state_1_old == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 483 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 484 | } |
| smilestone520 | 10:06cc2b1d2aaf | 485 | state_1_old = state_1; |
| smilestone520 | 10:06cc2b1d2aaf | 486 | |
| smilestone520 | 10:06cc2b1d2aaf | 487 | |
| smilestone520 | 10:06cc2b1d2aaf | 488 | ////////////////////////////////// |
| smilestone520 | 10:06cc2b1d2aaf | 489 | |
| smilestone520 | 10:06cc2b1d2aaf | 490 | //Forward |
| smilestone520 | 10:06cc2b1d2aaf | 491 | //v1Count +1 |
| smilestone520 | 10:06cc2b1d2aaf | 492 | //Inverse |
| smilestone520 | 10:06cc2b1d2aaf | 493 | //v1Count -1 |
| smilestone520 | 10:06cc2b1d2aaf | 494 | |
| smilestone520 | 10:06cc2b1d2aaf | 495 | |
| smilestone520 | 10:06cc2b1d2aaf | 496 | //Motor 2 |
| smilestone520 | 10:06cc2b1d2aaf | 497 | stateA_2 = HallA_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 498 | stateB_2 = HallB_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 499 | |
| smilestone520 | 10:06cc2b1d2aaf | 500 | ///code for state determination/// |
| smilestone520 | 10:06cc2b1d2aaf | 501 | if(stateA_2==0&&stateB_2==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 502 | state_2 = 1;} |
| smilestone520 | 10:06cc2b1d2aaf | 503 | else if(stateA_2==0&&stateB_2==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 504 | state_2 = 2;} |
| smilestone520 | 10:06cc2b1d2aaf | 505 | else if(stateA_2==1&&stateB_2==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 506 | state_2 = 3;} |
| smilestone520 | 10:06cc2b1d2aaf | 507 | else if(stateA_2==1&&stateB_2==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 508 | state_2 = 4;} |
| smilestone520 | 10:06cc2b1d2aaf | 509 | |
| smilestone520 | 10:06cc2b1d2aaf | 510 | if(state_2 == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 511 | { |
| smilestone520 | 10:06cc2b1d2aaf | 512 | if(state_2-state_2_old == -3) |
| smilestone520 | 10:06cc2b1d2aaf | 513 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 514 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 515 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 516 | } |
| smilestone520 | 10:06cc2b1d2aaf | 517 | else if(state_2 == 2) |
| smilestone520 | 10:06cc2b1d2aaf | 518 | { |
| smilestone520 | 10:06cc2b1d2aaf | 519 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 520 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 521 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 522 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 523 | } |
| smilestone520 | 10:06cc2b1d2aaf | 524 | else if(state_2 == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 525 | { |
| smilestone520 | 10:06cc2b1d2aaf | 526 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 527 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 528 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 529 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 530 | } |
| smilestone520 | 10:06cc2b1d2aaf | 531 | else if(state_2 == 4) |
| smilestone520 | 10:06cc2b1d2aaf | 532 | { |
| smilestone520 | 10:06cc2b1d2aaf | 533 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 534 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 535 | else if(state_2-state_2_old == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 536 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 537 | } |
| smilestone520 | 10:06cc2b1d2aaf | 538 | state_2_old = state_2; |
| smilestone520 | 10:06cc2b1d2aaf | 539 | |
| smilestone520 | 10:06cc2b1d2aaf | 540 | ////////////////////////////////// |
| smilestone520 | 10:06cc2b1d2aaf | 541 | |
| smilestone520 | 10:06cc2b1d2aaf | 542 | //Forward |
| smilestone520 | 10:06cc2b1d2aaf | 543 | //v2Count +1 |
| smilestone520 | 10:06cc2b1d2aaf | 544 | //Inverse |
| smilestone520 | 10:06cc2b1d2aaf | 545 | //v2Count -1 |
| smilestone520 | 10:06cc2b1d2aaf | 546 | } |
| smilestone520 | 10:06cc2b1d2aaf | 547 | |
| roger5641 | 17:d96afd9d2692 | 548 | /*void _ISR_U2RXInterrupt(void) |
| roger5641 | 15:3e0a4079b28a | 549 | { |
| roger5641 | 15:3e0a4079b28a | 550 | /////////// Receive //////////// |
| roger5641 | 15:3e0a4079b28a | 551 | static char Temp; |
| roger5641 | 15:3e0a4079b28a | 552 | Temp = U2RXREG; |
| roger5641 | 15:3e0a4079b28a | 553 | |
| roger5641 | 15:3e0a4079b28a | 554 | if(Receive_Flag == 1) |
| roger5641 | 15:3e0a4079b28a | 555 | { |
| roger5641 | 15:3e0a4079b28a | 556 | Receive_Counter++; |
| roger5641 | 15:3e0a4079b28a | 557 | Receive_Data[Receive_Counter] = Temp; |
| roger5641 | 15:3e0a4079b28a | 558 | |
| roger5641 | 15:3e0a4079b28a | 559 | if(Receive_Counter == 33) // 8 data *4byte |
| roger5641 | 15:3e0a4079b28a | 560 | { |
| roger5641 | 15:3e0a4079b28a | 561 | //Send_Flag == 1 |
| roger5641 | 15:3e0a4079b28a | 562 | Command_Flag = 1; |
| roger5641 | 15:3e0a4079b28a | 563 | Receive_Flag = 0; |
| roger5641 | 15:3e0a4079b28a | 564 | Receive_Counter = 0; |
| roger5641 | 15:3e0a4079b28a | 565 | } |
| roger5641 | 15:3e0a4079b28a | 566 | } |
| roger5641 | 15:3e0a4079b28a | 567 | |
| roger5641 | 15:3e0a4079b28a | 568 | else |
| roger5641 | 15:3e0a4079b28a | 569 | { |
| roger5641 | 15:3e0a4079b28a | 570 | if(Temp == 36) //'$' |
| roger5641 | 15:3e0a4079b28a | 571 | { |
| roger5641 | 15:3e0a4079b28a | 572 | Receive_Flag = 1; |
| roger5641 | 15:3e0a4079b28a | 573 | Receive_Counter = 0; |
| roger5641 | 15:3e0a4079b28a | 574 | Receive_Data[0] = Temp; |
| roger5641 | 15:3e0a4079b28a | 575 | } |
| roger5641 | 15:3e0a4079b28a | 576 | else |
| roger5641 | 15:3e0a4079b28a | 577 | { |
| roger5641 | 15:3e0a4079b28a | 578 | // Waiting |
| roger5641 | 15:3e0a4079b28a | 579 | } |
| roger5641 | 15:3e0a4079b28a | 580 | } |
| roger5641 | 15:3e0a4079b28a | 581 | |
| roger5641 | 15:3e0a4079b28a | 582 | IFS1bits.U2RXIF = 0; |
| roger5641 | 15:3e0a4079b28a | 583 | } |
| roger5641 | 17:d96afd9d2692 | 584 | */ |
| roger5641 | 15:3e0a4079b28a | 585 | |
| smilestone520 | 10:06cc2b1d2aaf | 586 | void init_TIMER(void) |
| smilestone520 | 10:06cc2b1d2aaf | 587 | { |
| smilestone520 | 10:06cc2b1d2aaf | 588 | timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) |
| smilestone520 | 10:06cc2b1d2aaf | 589 | } |
| smilestone520 | 10:06cc2b1d2aaf | 590 | |
| smilestone520 | 10:06cc2b1d2aaf | 591 | void init_PWM(void) |
| smilestone520 | 10:06cc2b1d2aaf | 592 | { |
| smilestone520 | 10:06cc2b1d2aaf | 593 | pwm1.period_us(50); |
| smilestone520 | 10:06cc2b1d2aaf | 594 | pwm1.write(0.5); |
| smilestone520 | 10:06cc2b1d2aaf | 595 | TIM1->CCER |= 0x4; |
| smilestone520 | 10:06cc2b1d2aaf | 596 | |
| smilestone520 | 10:06cc2b1d2aaf | 597 | pwm2.period_us(50); |
| smilestone520 | 10:06cc2b1d2aaf | 598 | pwm2.write(0.5); |
| smilestone520 | 10:06cc2b1d2aaf | 599 | TIM1->CCER |= 0x40; |
| smilestone520 | 10:06cc2b1d2aaf | 600 | } |
| smilestone520 | 10:06cc2b1d2aaf | 601 | |
| smilestone520 | 10:06cc2b1d2aaf | 602 | void init_CN(void) |
| smilestone520 | 10:06cc2b1d2aaf | 603 | { |
| smilestone520 | 10:06cc2b1d2aaf | 604 | HallA_1.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 605 | HallA_1.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 606 | HallB_1.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 607 | HallB_1.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 608 | |
| smilestone520 | 10:06cc2b1d2aaf | 609 | HallA_2.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 610 | HallA_2.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 611 | HallB_2.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 612 | HallB_2.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 613 | |
| smilestone520 | 10:06cc2b1d2aaf | 614 | stateA_1 = HallA_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 615 | stateB_1 = HallB_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 616 | stateA_2 = HallA_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 617 | stateB_2 = HallB_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 618 | } |