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