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@20:5b892e37a958, 2016-05-25 (annotated)
- Committer:
- smilestone520
- Date:
- Wed May 25 16:43:21 2016 +0000
- Revision:
- 20:5b892e37a958
- Parent:
- 19:5091c934ebd0
- Child:
- 21:6e8ab9487985
hahahaha
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 | 20:5b892e37a958 | 38 | void funcBorder(void); |
| smilestone520 | 20:5b892e37a958 | 39 | void smallAngle( float ); |
| smilestone520 | 20:5b892e37a958 | 40 | void turnCW(float); |
| smilestone520 | 20:5b892e37a958 | 41 | void turnCCW(float); |
| smilestone520 | 10:06cc2b1d2aaf | 42 | |
| smilestone520 | 10:06cc2b1d2aaf | 43 | int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0; |
| smilestone520 | 10:06cc2b1d2aaf | 44 | int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 45 | |
| smilestone520 | 10:06cc2b1d2aaf | 46 | int v1Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 47 | int v2Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 48 | |
| roger5641 | 17:d96afd9d2692 | 49 | float v1 = 0.0, v1_ref = 0.0; |
| smilestone520 | 10:06cc2b1d2aaf | 50 | float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0; |
| roger5641 | 17:d96afd9d2692 | 51 | float v2 = 0.0, v2_ref = 0.0; |
| smilestone520 | 10:06cc2b1d2aaf | 52 | float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0; |
| roger5641 | 17:d96afd9d2692 | 53 | |
| roger5641 | 17:d96afd9d2692 | 54 | int angle = 90; |
| roger5641 | 15:3e0a4079b28a | 55 | float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90 |
| roger5641 | 15:3e0a4079b28a | 56 | |
| roger5641 | 18:2db6c97a4145 | 57 | int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0; |
| roger5641 | 18:2db6c97a4145 | 58 | int Receive_Data[33] = {0}; |
| roger5641 | 15:3e0a4079b28a | 59 | |
| roger5641 | 18:2db6c97a4145 | 60 | double Distance_Target = 0, Angle_Target = 0; |
| roger5641 | 15:3e0a4079b28a | 61 | int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0; |
| roger5641 | 15:3e0a4079b28a | 62 | int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0; |
| roger5641 | 15:3e0a4079b28a | 63 | int pwm_duty; |
| roger5641 | 15:3e0a4079b28a | 64 | |
| smilestone520 | 20:5b892e37a958 | 65 | float k_forward = 20, k_turn = 10; |
| smilestone520 | 20:5b892e37a958 | 66 | |
| smilestone520 | 11:44989c0bcea5 | 67 | //**** receive and return by bluetooth ************** // bluetooth.getc() |
| smilestone520 | 14:c46f22bcaa38 | 68 | float xC, yC; // car's position |
| smilestone520 | 14:c46f22bcaa38 | 69 | float angleC;// car's angle |
| roger5641 | 17:d96afd9d2692 | 70 | double xP,yP,distance; //position that car need to reach |
| smilestone520 | 14:c46f22bcaa38 | 71 | float angleR; // angleR = car-nextSpot direction ---> car direction |
| smilestone520 | 20:5b892e37a958 | 72 | int yB1,yB2,xB3,xB4; // broder conditions |
| roger5641 | 17:d96afd9d2692 | 73 | int xGate = 800, yGate = 300; |
| roger5641 | 17:d96afd9d2692 | 74 | double angleGate, distanceGate; |
| smilestone520 | 11:44989c0bcea5 | 75 | int aI_State = 0; |
| roger5641 | 17:d96afd9d2692 | 76 | double pi = 3.1415926; |
| smilestone520 | 11:44989c0bcea5 | 77 | |
| smilestone520 | 20:5b892e37a958 | 78 | float longC; // car's length |
| smilestone520 | 20:5b892e37a958 | 79 | float longB;// long of the court |
| smilestone520 | 20:5b892e37a958 | 80 | float ballSize; // ball size |
| smilestone520 | 20:5b892e37a958 | 81 | float wideB; // wide of the court |
| smilestone520 | 20:5b892e37a958 | 82 | |
| smilestone520 | 10:06cc2b1d2aaf | 83 | int main() { |
| smilestone520 | 10:06cc2b1d2aaf | 84 | |
| smilestone520 | 10:06cc2b1d2aaf | 85 | init_TIMER(); |
| smilestone520 | 10:06cc2b1d2aaf | 86 | init_PWM(); |
| smilestone520 | 10:06cc2b1d2aaf | 87 | init_CN(); |
| roger5641 | 17:d96afd9d2692 | 88 | servo.write(0.079 + (0.084/180)*angle); |
| smilestone520 | 10:06cc2b1d2aaf | 89 | |
| smilestone520 | 10:06cc2b1d2aaf | 90 | bluetooth.baud(115200); //設定鮑率 |
| smilestone520 | 10:06cc2b1d2aaf | 91 | pc.baud(57600); |
| smilestone520 | 10:06cc2b1d2aaf | 92 | |
| smilestone520 | 10:06cc2b1d2aaf | 93 | |
| smilestone520 | 10:06cc2b1d2aaf | 94 | while(1) |
| smilestone520 | 10:06cc2b1d2aaf | 95 | { |
| smilestone520 | 10:06cc2b1d2aaf | 96 | if(pc.readable()) |
| smilestone520 | 10:06cc2b1d2aaf | 97 | { |
| smilestone520 | 10:06cc2b1d2aaf | 98 | bluetooth.putc(pc.getc()); |
| smilestone520 | 10:06cc2b1d2aaf | 99 | } |
| smilestone520 | 10:06cc2b1d2aaf | 100 | if(bluetooth.readable()) |
| smilestone520 | 10:06cc2b1d2aaf | 101 | { |
| smilestone520 | 10:06cc2b1d2aaf | 102 | pc.putc(bluetooth.getc()); |
| smilestone520 | 10:06cc2b1d2aaf | 103 | } |
| roger5641 | 15:3e0a4079b28a | 104 | |
| roger5641 | 19:5091c934ebd0 | 105 | if(Command_Flag == 1) |
| roger5641 | 15:3e0a4079b28a | 106 | { |
| roger5641 | 15:3e0a4079b28a | 107 | //read data from matlab |
| roger5641 | 15:3e0a4079b28a | 108 | //distance_target |
| roger5641 | 15:3e0a4079b28a | 109 | Distance_Target = (Receive_Data[2]-0x30)*100 + (Receive_Data[3]-0x30)*10 + (Receive_Data[4]-0x30); |
| roger5641 | 15:3e0a4079b28a | 110 | |
| roger5641 | 15:3e0a4079b28a | 111 | if(Receive_Data[1] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 112 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 113 | |
| roger5641 | 15:3e0a4079b28a | 114 | //ang_rel_target |
| roger5641 | 15:3e0a4079b28a | 115 | Angle_Target = (Receive_Data[6]-0x30)*100 + (Receive_Data[7]-0x30)*10 + (Receive_Data[8]-0x30); |
| roger5641 | 15:3e0a4079b28a | 116 | |
| roger5641 | 15:3e0a4079b28a | 117 | if(Receive_Data[5] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 118 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 119 | |
| roger5641 | 15:3e0a4079b28a | 120 | //x_position_car_1 |
| roger5641 | 15:3e0a4079b28a | 121 | X_Position_1 = (Receive_Data[10]-0x30)*100 + (Receive_Data[11]-0x30)*10 + (Receive_Data[12]-0x30); |
| roger5641 | 15:3e0a4079b28a | 122 | |
| roger5641 | 15:3e0a4079b28a | 123 | if(Receive_Data[9] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 124 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 125 | |
| roger5641 | 15:3e0a4079b28a | 126 | //y_position_car_1 |
| roger5641 | 15:3e0a4079b28a | 127 | Y_Position_1 = (Receive_Data[14]-0x30)*100 + (Receive_Data[15]-0x30)*10 + (Receive_Data[16]-0x30); |
| roger5641 | 15:3e0a4079b28a | 128 | |
| roger5641 | 15:3e0a4079b28a | 129 | if(Receive_Data[13] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 130 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 131 | |
| roger5641 | 15:3e0a4079b28a | 132 | //angle_car_1 |
| roger5641 | 15:3e0a4079b28a | 133 | Angle_1 = (Receive_Data[18]-0x30)*100 + (Receive_Data[19]-0x30)*10 + (Receive_Data[20]-0x30); |
| roger5641 | 15:3e0a4079b28a | 134 | |
| roger5641 | 15:3e0a4079b28a | 135 | if(Receive_Data[17] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 136 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 137 | |
| roger5641 | 15:3e0a4079b28a | 138 | //x_position_car_2 |
| roger5641 | 15:3e0a4079b28a | 139 | X_Position_2 = (Receive_Data[22]-0x30)*100 + (Receive_Data[23]-0x30)*10 + (Receive_Data[24]-0x30); |
| roger5641 | 15:3e0a4079b28a | 140 | |
| roger5641 | 15:3e0a4079b28a | 141 | if(Receive_Data[21] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 142 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 143 | |
| roger5641 | 15:3e0a4079b28a | 144 | //y_position_car_2 |
| roger5641 | 15:3e0a4079b28a | 145 | Y_Position_2 = (Receive_Data[26]-0x30)*100 + (Receive_Data[27]-0x30)*10 + (Receive_Data[28]-0x30); |
| roger5641 | 15:3e0a4079b28a | 146 | |
| roger5641 | 15:3e0a4079b28a | 147 | if(Receive_Data[25] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 148 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 149 | |
| roger5641 | 15:3e0a4079b28a | 150 | //angle_car_1 |
| roger5641 | 15:3e0a4079b28a | 151 | Angle_2 = (Receive_Data[30]-0x30)*100 + (Receive_Data[31]-0x30)*10 + (Receive_Data[32]-0x30); |
| roger5641 | 15:3e0a4079b28a | 152 | |
| roger5641 | 15:3e0a4079b28a | 153 | if(Receive_Data[29] == '-')Distance_Target = -1* Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 154 | else Distance_Target = Distance_Target; |
| roger5641 | 15:3e0a4079b28a | 155 | |
| roger5641 | 15:3e0a4079b28a | 156 | // PWM |
| roger5641 | 15:3e0a4079b28a | 157 | if(Receive_Data[33] == 'j')pwm_duty = 148; |
| roger5641 | 15:3e0a4079b28a | 158 | else if(Receive_Data[33] == 'k')pwm_duty = 100; |
| roger5641 | 15:3e0a4079b28a | 159 | |
| roger5641 | 15:3e0a4079b28a | 160 | Command_Flag = 0; |
| roger5641 | 19:5091c934ebd0 | 161 | } |
| smilestone520 | 10:06cc2b1d2aaf | 162 | } |
| smilestone520 | 10:06cc2b1d2aaf | 163 | } |
| smilestone520 | 10:06cc2b1d2aaf | 164 | |
| smilestone520 | 10:06cc2b1d2aaf | 165 | void timer1_interrupt(void) |
| smilestone520 | 10:06cc2b1d2aaf | 166 | { |
| smilestone520 | 10:06cc2b1d2aaf | 167 | //Motor 1 |
| smilestone520 | 10:06cc2b1d2aaf | 168 | v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
| smilestone520 | 10:06cc2b1d2aaf | 169 | v1Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 170 | |
| smilestone520 | 10:06cc2b1d2aaf | 171 | ///code for PI control/// |
| smilestone520 | 10:06cc2b1d2aaf | 172 | v1_err = v1_ref - v1; |
| smilestone520 | 10:06cc2b1d2aaf | 173 | v1_ierr = 0.01f*v1_err + v1_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 174 | PIout_1 = Kp*v1_err + Ki*v1_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 175 | |
| smilestone520 | 10:06cc2b1d2aaf | 176 | if(PIout_1 >= 0.5f)PIout_1 = 0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 177 | else if(PIout_1 <= -0.5f)PIout_1 = -0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 178 | pwm1.write(PIout_1 + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 179 | TIM1->CCER |= 0x4; |
| smilestone520 | 10:06cc2b1d2aaf | 180 | |
| smilestone520 | 10:06cc2b1d2aaf | 181 | //Motor 2 |
| smilestone520 | 10:06cc2b1d2aaf | 182 | v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
| smilestone520 | 10:06cc2b1d2aaf | 183 | v2Count = 0; |
| smilestone520 | 10:06cc2b1d2aaf | 184 | |
| smilestone520 | 10:06cc2b1d2aaf | 185 | ///code for PI control/// |
| smilestone520 | 10:06cc2b1d2aaf | 186 | v2_err = v2_ref - v2; |
| smilestone520 | 10:06cc2b1d2aaf | 187 | v2_ierr = 0.01f*v2_err + v2_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 188 | PIout_2 = Kp*v2_err + Ki*v2_ierr; |
| smilestone520 | 10:06cc2b1d2aaf | 189 | |
| smilestone520 | 10:06cc2b1d2aaf | 190 | if(PIout_2 >= 0.5f)PIout_2 = 0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 191 | else if(PIout_2 <= -0.5f)PIout_2 = -0.5f; |
| smilestone520 | 10:06cc2b1d2aaf | 192 | pwm2.write(PIout_2 + 0.5f); |
| smilestone520 | 10:06cc2b1d2aaf | 193 | TIM1->CCER |= 0x40; |
| smilestone520 | 10:06cc2b1d2aaf | 194 | |
| roger5641 | 17:d96afd9d2692 | 195 | |
| smilestone520 | 11:44989c0bcea5 | 196 | //***** main AI ************************** |
| smilestone520 | 11:44989c0bcea5 | 197 | |
| smilestone520 | 11:44989c0bcea5 | 198 | |
| smilestone520 | 11:44989c0bcea5 | 199 | //***** get position information from the bluetooth |
| roger5641 | 17:d96afd9d2692 | 200 | xC = X_Position_1; |
| roger5641 | 17:d96afd9d2692 | 201 | yC = Y_Position_1; // car's position |
| smilestone520 | 11:44989c0bcea5 | 202 | |
| roger5641 | 17:d96afd9d2692 | 203 | angleC = Angle_1; // car's angle |
| roger5641 | 17:d96afd9d2692 | 204 | |
| roger5641 | 17:d96afd9d2692 | 205 | distance = Distance_Target; |
| roger5641 | 18:2db6c97a4145 | 206 | xP = Distance_Target*cos(Angle_Target*pi/180); |
| roger5641 | 18:2db6c97a4145 | 207 | yP = Distance_Target*sin(Angle_Target*pi/180);//position that car need to reach |
| smilestone520 | 11:44989c0bcea5 | 208 | |
| roger5641 | 17:d96afd9d2692 | 209 | angleR = Angle_Target; // angleR = car-nextSpot direction ---> car direction |
| smilestone520 | 11:44989c0bcea5 | 210 | |
| smilestone520 | 20:5b892e37a958 | 211 | yB1 = longC; |
| smilestone520 | 20:5b892e37a958 | 212 | yB2 = wideB - longC ; |
| smilestone520 | 20:5b892e37a958 | 213 | xB3 = longC ; |
| smilestone520 | 20:5b892e37a958 | 214 | xB4 = longB-longC; |
| smilestone520 | 20:5b892e37a958 | 215 | |
| smilestone520 | 11:44989c0bcea5 | 216 | //**** AI_State ******************** |
| smilestone520 | 11:44989c0bcea5 | 217 | |
| smilestone520 | 11:44989c0bcea5 | 218 | switch(aI_State) |
| smilestone520 | 11:44989c0bcea5 | 219 | { |
| smilestone520 | 12:c1a667ca6c53 | 220 | case 0: // IDLE |
| smilestone520 | 12:c1a667ca6c53 | 221 | // IDLE check if stop color appear |
| smilestone520 | 12:c1a667ca6c53 | 222 | //check purple color appear or not |
| roger5641 | 17:d96afd9d2692 | 223 | if(xP==0 && yP==0 && angleR==0) |
| roger5641 | 17:d96afd9d2692 | 224 | { |
| roger5641 | 17:d96afd9d2692 | 225 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 226 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 227 | } |
| roger5641 | 17:d96afd9d2692 | 228 | else |
| roger5641 | 17:d96afd9d2692 | 229 | aI_State = 1; |
| smilestone520 | 14:c46f22bcaa38 | 230 | //****setSpecs(); |
| smilestone520 | 11:44989c0bcea5 | 231 | break; |
| smilestone520 | 16:a102929b2228 | 232 | |
| roger5641 | 17:d96afd9d2692 | 233 | case 1: /// border condition |
| smilestone520 | 12:c1a667ca6c53 | 234 | // check if car fit border conditions |
| smilestone520 | 20:5b892e37a958 | 235 | funcBorder(); |
| roger5641 | 17:d96afd9d2692 | 236 | //****to case2 |
| smilestone520 | 11:44989c0bcea5 | 237 | break; |
| roger5641 | 17:d96afd9d2692 | 238 | case 2://move to get ball |
| roger5641 | 17:d96afd9d2692 | 239 | if(-3<=angleR<=3) // direct to the ball |
| roger5641 | 17:d96afd9d2692 | 240 | { |
| roger5641 | 17:d96afd9d2692 | 241 | if(distance<=10) // check夾具距離 |
| roger5641 | 17:d96afd9d2692 | 242 | { |
| roger5641 | 17:d96afd9d2692 | 243 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 244 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 245 | aI_State = 3; |
| roger5641 | 17:d96afd9d2692 | 246 | } |
| roger5641 | 17:d96afd9d2692 | 247 | else if(10<distance<50) |
| roger5641 | 17:d96afd9d2692 | 248 | { |
| roger5641 | 17:d96afd9d2692 | 249 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 250 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 251 | } |
| roger5641 | 17:d96afd9d2692 | 252 | else |
| roger5641 | 17:d96afd9d2692 | 253 | { |
| roger5641 | 17:d96afd9d2692 | 254 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 255 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 256 | } |
| roger5641 | 17:d96afd9d2692 | 257 | } |
| roger5641 | 17:d96afd9d2692 | 258 | |
| roger5641 | 17:d96afd9d2692 | 259 | else if(3<angleR<=15) // small angle right |
| roger5641 | 17:d96afd9d2692 | 260 | { |
| roger5641 | 17:d96afd9d2692 | 261 | if(distance<50) |
| roger5641 | 17:d96afd9d2692 | 262 | { |
| roger5641 | 17:d96afd9d2692 | 263 | v1_ref = 200; |
| smilestone520 | 20:5b892e37a958 | 264 | v2_ref = -(v1_ref-angleR*k_turn); |
| roger5641 | 17:d96afd9d2692 | 265 | } |
| roger5641 | 17:d96afd9d2692 | 266 | else |
| roger5641 | 17:d96afd9d2692 | 267 | { |
| roger5641 | 17:d96afd9d2692 | 268 | v1_ref = 300; |
| smilestone520 | 20:5b892e37a958 | 269 | v2_ref = -(v1_ref-angleR*k_turn); |
| roger5641 | 17:d96afd9d2692 | 270 | } |
| roger5641 | 17:d96afd9d2692 | 271 | |
| roger5641 | 17:d96afd9d2692 | 272 | } |
| smilestone520 | 20:5b892e37a958 | 273 | else if(-15 <= angleR < -3) // small angle left |
| roger5641 | 17:d96afd9d2692 | 274 | { |
| roger5641 | 17:d96afd9d2692 | 275 | if(distance<50) |
| roger5641 | 17:d96afd9d2692 | 276 | { |
| smilestone520 | 20:5b892e37a958 | 277 | v1_ref = -(v2_ref+angleR*k_turn); |
| roger5641 | 17:d96afd9d2692 | 278 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 279 | } |
| roger5641 | 17:d96afd9d2692 | 280 | else |
| roger5641 | 17:d96afd9d2692 | 281 | { |
| smilestone520 | 20:5b892e37a958 | 282 | v1_ref = -(v2_ref+angleR*k_turn); |
| 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(15<angleR<=180) // big angle right |
| 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(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 | else if(-180<=angleR<-15) // big angle left |
| roger5641 | 17:d96afd9d2692 | 305 | { |
| roger5641 | 17:d96afd9d2692 | 306 | v1_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 307 | v2_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 308 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 309 | if(0<distance<50) |
| roger5641 | 17:d96afd9d2692 | 310 | { |
| roger5641 | 17:d96afd9d2692 | 311 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 312 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 313 | } |
| roger5641 | 17:d96afd9d2692 | 314 | else |
| roger5641 | 17:d96afd9d2692 | 315 | { |
| roger5641 | 17:d96afd9d2692 | 316 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 317 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 318 | } |
| roger5641 | 17:d96afd9d2692 | 319 | |
| roger5641 | 17:d96afd9d2692 | 320 | } |
| roger5641 | 17:d96afd9d2692 | 321 | |
| roger5641 | 17:d96afd9d2692 | 322 | break; |
| roger5641 | 17:d96afd9d2692 | 323 | case 3: //****getBall(); |
| roger5641 | 17:d96afd9d2692 | 324 | if((distance == 10) && (-3<=angleR<=3)) |
| roger5641 | 17:d96afd9d2692 | 325 | { |
| roger5641 | 17:d96afd9d2692 | 326 | angle = -10; |
| roger5641 | 17:d96afd9d2692 | 327 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 328 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 329 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 330 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 331 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 332 | } |
| roger5641 | 17:d96afd9d2692 | 333 | else |
| roger5641 | 17:d96afd9d2692 | 334 | aI_State = 2; |
| roger5641 | 17:d96afd9d2692 | 335 | |
| smilestone520 | 11:44989c0bcea5 | 336 | break; |
| smilestone520 | 12:c1a667ca6c53 | 337 | case 4: /// move to the gate and release ball |
| smilestone520 | 12:c1a667ca6c53 | 338 | // move to the point in front of the gate first |
| smilestone520 | 12:c1a667ca6c53 | 339 | // then head to the gate |
| roger5641 | 18:2db6c97a4145 | 340 | distanceGate = sqrt(((xGate-xP)*(xGate-xP))+((yGate-yP)*(yGate-yP))); |
| roger5641 | 17:d96afd9d2692 | 341 | angleGate = atan2((yGate-yP),(xGate-xP))*180/pi; |
| roger5641 | 17:d96afd9d2692 | 342 | if(-3<=angleGate<=3) // direct to the gate |
| roger5641 | 17:d96afd9d2692 | 343 | { |
| roger5641 | 17:d96afd9d2692 | 344 | if(distanceGate<=10) // check distance between car cemtroid and gate |
| roger5641 | 17:d96afd9d2692 | 345 | { |
| roger5641 | 17:d96afd9d2692 | 346 | v1_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 347 | v2_ref = 0; |
| roger5641 | 17:d96afd9d2692 | 348 | aI_State = 5; |
| roger5641 | 17:d96afd9d2692 | 349 | } |
| roger5641 | 17:d96afd9d2692 | 350 | else if(10<distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 351 | { |
| roger5641 | 17:d96afd9d2692 | 352 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 353 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 354 | } |
| roger5641 | 17:d96afd9d2692 | 355 | else |
| roger5641 | 17:d96afd9d2692 | 356 | { |
| roger5641 | 17:d96afd9d2692 | 357 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 358 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 359 | } |
| roger5641 | 17:d96afd9d2692 | 360 | } |
| roger5641 | 17:d96afd9d2692 | 361 | else if(3<angleGate<=15) // small angle right |
| roger5641 | 17:d96afd9d2692 | 362 | { |
| roger5641 | 17:d96afd9d2692 | 363 | if(distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 364 | { |
| roger5641 | 17:d96afd9d2692 | 365 | v1_ref = 200; |
| smilestone520 | 20:5b892e37a958 | 366 | v2_ref = -(v1_ref-angleGate*k_turn); |
| roger5641 | 17:d96afd9d2692 | 367 | } |
| roger5641 | 17:d96afd9d2692 | 368 | else |
| roger5641 | 17:d96afd9d2692 | 369 | { |
| roger5641 | 17:d96afd9d2692 | 370 | v1_ref = 300; |
| smilestone520 | 20:5b892e37a958 | 371 | v2_ref = -(v1_ref-angleGate*k_turn); |
| roger5641 | 17:d96afd9d2692 | 372 | } |
| roger5641 | 17:d96afd9d2692 | 373 | } |
| roger5641 | 17:d96afd9d2692 | 374 | else if(-15<=angleGate<-3) // small angle left |
| roger5641 | 17:d96afd9d2692 | 375 | { |
| roger5641 | 17:d96afd9d2692 | 376 | if(distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 377 | { |
| smilestone520 | 20:5b892e37a958 | 378 | v1_ref = -(v2_ref+angleGate*k_turn); |
| roger5641 | 17:d96afd9d2692 | 379 | v2_ref = 200; |
| roger5641 | 17:d96afd9d2692 | 380 | } |
| roger5641 | 17:d96afd9d2692 | 381 | else |
| roger5641 | 17:d96afd9d2692 | 382 | { |
| smilestone520 | 20:5b892e37a958 | 383 | v1_ref = -(v2_ref+angleGate*k_turn); |
| roger5641 | 17:d96afd9d2692 | 384 | v2_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 385 | } |
| roger5641 | 17:d96afd9d2692 | 386 | } |
| roger5641 | 17:d96afd9d2692 | 387 | else if(15<angleGate<=180) // big angle right |
| 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(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 | else if(-180<=angleGate<-15) // big angle left |
| roger5641 | 17:d96afd9d2692 | 404 | { |
| roger5641 | 17:d96afd9d2692 | 405 | v1_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 406 | v2_ref = -200; |
| roger5641 | 17:d96afd9d2692 | 407 | wait(10); //need to count the relation between angle and time in v = 200 |
| roger5641 | 17:d96afd9d2692 | 408 | if(0<distanceGate<50) |
| roger5641 | 17:d96afd9d2692 | 409 | { |
| roger5641 | 17:d96afd9d2692 | 410 | v1_ref = 100; |
| roger5641 | 17:d96afd9d2692 | 411 | v2_ref = -100; |
| roger5641 | 17:d96afd9d2692 | 412 | } |
| roger5641 | 17:d96afd9d2692 | 413 | else |
| roger5641 | 17:d96afd9d2692 | 414 | { |
| roger5641 | 17:d96afd9d2692 | 415 | v1_ref = 300; |
| roger5641 | 17:d96afd9d2692 | 416 | v2_ref = -300; |
| roger5641 | 17:d96afd9d2692 | 417 | } |
| roger5641 | 17:d96afd9d2692 | 418 | } |
| roger5641 | 17:d96afd9d2692 | 419 | |
| smilestone520 | 12:c1a667ca6c53 | 420 | break; |
| roger5641 | 17:d96afd9d2692 | 421 | case 5: // release ball |
| roger5641 | 17:d96afd9d2692 | 422 | if((-3<=angleGate<=3) && (distanceGate<=10)) // direct to the gate |
| roger5641 | 17:d96afd9d2692 | 423 | { |
| roger5641 | 17:d96afd9d2692 | 424 | angle = 90; // Fixture up |
| roger5641 | 17:d96afd9d2692 | 425 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 426 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 427 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 428 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 429 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 430 | |
| roger5641 | 17:d96afd9d2692 | 431 | pwm1.write(0.1f + 0.5f); // push |
| roger5641 | 17:d96afd9d2692 | 432 | pwm2.write(-0.1f + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 433 | wait(0.5); |
| roger5641 | 17:d96afd9d2692 | 434 | |
| roger5641 | 17:d96afd9d2692 | 435 | angle = -10; // Fixture down |
| roger5641 | 17:d96afd9d2692 | 436 | servo_duty = 0.079 + (0.084/180)*angle; |
| roger5641 | 17:d96afd9d2692 | 437 | servo.write(servo_duty); |
| roger5641 | 17:d96afd9d2692 | 438 | servo = 1; |
| roger5641 | 17:d96afd9d2692 | 439 | wait(0.1); |
| roger5641 | 17:d96afd9d2692 | 440 | servo = 0; |
| roger5641 | 17:d96afd9d2692 | 441 | |
| roger5641 | 17:d96afd9d2692 | 442 | pwm1.write(-0.3f + 0.5f); // back and leave gate |
| roger5641 | 17:d96afd9d2692 | 443 | pwm2.write(0.3f + 0.5f); |
| roger5641 | 17:d96afd9d2692 | 444 | wait(1); |
| roger5641 | 17:d96afd9d2692 | 445 | |
| roger5641 | 18:2db6c97a4145 | 446 | aI_State = 0; |
| roger5641 | 17:d96afd9d2692 | 447 | |
| roger5641 | 17:d96afd9d2692 | 448 | } |
| roger5641 | 17:d96afd9d2692 | 449 | |
| roger5641 | 17:d96afd9d2692 | 450 | break; |
| roger5641 | 17:d96afd9d2692 | 451 | |
| smilestone520 | 11:44989c0bcea5 | 452 | |
| smilestone520 | 11:44989c0bcea5 | 453 | } |
| smilestone520 | 11:44989c0bcea5 | 454 | |
| smilestone520 | 11:44989c0bcea5 | 455 | |
| smilestone520 | 10:06cc2b1d2aaf | 456 | } |
| smilestone520 | 10:06cc2b1d2aaf | 457 | |
| smilestone520 | 10:06cc2b1d2aaf | 458 | void CN_interrupt(void) |
| smilestone520 | 10:06cc2b1d2aaf | 459 | { |
| smilestone520 | 10:06cc2b1d2aaf | 460 | //Motor 1 |
| smilestone520 | 10:06cc2b1d2aaf | 461 | stateA_1 = HallA_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 462 | stateB_1 = HallB_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 463 | |
| smilestone520 | 10:06cc2b1d2aaf | 464 | ///code for state determination/// |
| smilestone520 | 10:06cc2b1d2aaf | 465 | if(stateA_1==0&&stateB_1==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 466 | state_1 = 1;} |
| smilestone520 | 10:06cc2b1d2aaf | 467 | else if(stateA_1==0&&stateB_1==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 468 | state_1 = 2;} |
| smilestone520 | 10:06cc2b1d2aaf | 469 | else if(stateA_1==1&&stateB_1==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 470 | state_1 = 3;} |
| smilestone520 | 10:06cc2b1d2aaf | 471 | else if(stateA_1==1&&stateB_1==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 472 | state_1 = 4;} |
| smilestone520 | 10:06cc2b1d2aaf | 473 | |
| smilestone520 | 10:06cc2b1d2aaf | 474 | if(state_1 == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 475 | { |
| smilestone520 | 10:06cc2b1d2aaf | 476 | if(state_1-state_1_old == -3) |
| smilestone520 | 10:06cc2b1d2aaf | 477 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 478 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 479 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 480 | } |
| smilestone520 | 10:06cc2b1d2aaf | 481 | else if(state_1 == 2) |
| smilestone520 | 10:06cc2b1d2aaf | 482 | { |
| smilestone520 | 10:06cc2b1d2aaf | 483 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 484 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 485 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 486 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 487 | } |
| smilestone520 | 10:06cc2b1d2aaf | 488 | else if(state_1 == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 489 | { |
| smilestone520 | 10:06cc2b1d2aaf | 490 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 491 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 492 | else if(state_1-state_1_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 493 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 494 | } |
| smilestone520 | 10:06cc2b1d2aaf | 495 | else if(state_1 == 4) |
| smilestone520 | 10:06cc2b1d2aaf | 496 | { |
| smilestone520 | 10:06cc2b1d2aaf | 497 | if(state_1-state_1_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 498 | v1Count=v1Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 499 | else if(state_1-state_1_old == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 500 | v1Count=v1Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 501 | } |
| smilestone520 | 10:06cc2b1d2aaf | 502 | state_1_old = state_1; |
| smilestone520 | 10:06cc2b1d2aaf | 503 | |
| smilestone520 | 10:06cc2b1d2aaf | 504 | |
| smilestone520 | 10:06cc2b1d2aaf | 505 | ////////////////////////////////// |
| smilestone520 | 10:06cc2b1d2aaf | 506 | |
| smilestone520 | 10:06cc2b1d2aaf | 507 | //Forward |
| smilestone520 | 10:06cc2b1d2aaf | 508 | //v1Count +1 |
| smilestone520 | 10:06cc2b1d2aaf | 509 | //Inverse |
| smilestone520 | 10:06cc2b1d2aaf | 510 | //v1Count -1 |
| smilestone520 | 10:06cc2b1d2aaf | 511 | |
| smilestone520 | 10:06cc2b1d2aaf | 512 | |
| smilestone520 | 10:06cc2b1d2aaf | 513 | //Motor 2 |
| smilestone520 | 10:06cc2b1d2aaf | 514 | stateA_2 = HallA_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 515 | stateB_2 = HallB_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 516 | |
| smilestone520 | 10:06cc2b1d2aaf | 517 | ///code for state determination/// |
| smilestone520 | 10:06cc2b1d2aaf | 518 | if(stateA_2==0&&stateB_2==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 519 | state_2 = 1;} |
| smilestone520 | 10:06cc2b1d2aaf | 520 | else if(stateA_2==0&&stateB_2==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 521 | state_2 = 2;} |
| smilestone520 | 10:06cc2b1d2aaf | 522 | else if(stateA_2==1&&stateB_2==1){ |
| smilestone520 | 10:06cc2b1d2aaf | 523 | state_2 = 3;} |
| smilestone520 | 10:06cc2b1d2aaf | 524 | else if(stateA_2==1&&stateB_2==0){ |
| smilestone520 | 10:06cc2b1d2aaf | 525 | state_2 = 4;} |
| smilestone520 | 10:06cc2b1d2aaf | 526 | |
| smilestone520 | 10:06cc2b1d2aaf | 527 | if(state_2 == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 528 | { |
| smilestone520 | 10:06cc2b1d2aaf | 529 | if(state_2-state_2_old == -3) |
| smilestone520 | 10:06cc2b1d2aaf | 530 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 531 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 532 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 533 | } |
| smilestone520 | 10:06cc2b1d2aaf | 534 | else if(state_2 == 2) |
| smilestone520 | 10:06cc2b1d2aaf | 535 | { |
| smilestone520 | 10:06cc2b1d2aaf | 536 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 537 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 538 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 539 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 540 | } |
| smilestone520 | 10:06cc2b1d2aaf | 541 | else if(state_2 == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 542 | { |
| smilestone520 | 10:06cc2b1d2aaf | 543 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 544 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 545 | else if(state_2-state_2_old == -1) |
| smilestone520 | 10:06cc2b1d2aaf | 546 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 547 | } |
| smilestone520 | 10:06cc2b1d2aaf | 548 | else if(state_2 == 4) |
| smilestone520 | 10:06cc2b1d2aaf | 549 | { |
| smilestone520 | 10:06cc2b1d2aaf | 550 | if(state_2-state_2_old == 1) |
| smilestone520 | 10:06cc2b1d2aaf | 551 | v2Count=v2Count+1; |
| smilestone520 | 10:06cc2b1d2aaf | 552 | else if(state_2-state_2_old == 3) |
| smilestone520 | 10:06cc2b1d2aaf | 553 | v2Count=v2Count-1; |
| smilestone520 | 10:06cc2b1d2aaf | 554 | } |
| smilestone520 | 10:06cc2b1d2aaf | 555 | state_2_old = state_2; |
| smilestone520 | 10:06cc2b1d2aaf | 556 | |
| smilestone520 | 10:06cc2b1d2aaf | 557 | ////////////////////////////////// |
| smilestone520 | 10:06cc2b1d2aaf | 558 | |
| smilestone520 | 10:06cc2b1d2aaf | 559 | //Forward |
| smilestone520 | 10:06cc2b1d2aaf | 560 | //v2Count +1 |
| smilestone520 | 10:06cc2b1d2aaf | 561 | //Inverse |
| smilestone520 | 10:06cc2b1d2aaf | 562 | //v2Count -1 |
| smilestone520 | 10:06cc2b1d2aaf | 563 | } |
| smilestone520 | 10:06cc2b1d2aaf | 564 | |
| roger5641 | 17:d96afd9d2692 | 565 | /*void _ISR_U2RXInterrupt(void) |
| roger5641 | 15:3e0a4079b28a | 566 | { |
| roger5641 | 15:3e0a4079b28a | 567 | /////////// Receive //////////// |
| roger5641 | 15:3e0a4079b28a | 568 | static char Temp; |
| roger5641 | 15:3e0a4079b28a | 569 | Temp = U2RXREG; |
| roger5641 | 15:3e0a4079b28a | 570 | |
| roger5641 | 15:3e0a4079b28a | 571 | if(Receive_Flag == 1) |
| roger5641 | 15:3e0a4079b28a | 572 | { |
| roger5641 | 15:3e0a4079b28a | 573 | Receive_Counter++; |
| roger5641 | 15:3e0a4079b28a | 574 | Receive_Data[Receive_Counter] = Temp; |
| roger5641 | 15:3e0a4079b28a | 575 | |
| roger5641 | 15:3e0a4079b28a | 576 | if(Receive_Counter == 33) // 8 data *4byte |
| roger5641 | 15:3e0a4079b28a | 577 | { |
| roger5641 | 15:3e0a4079b28a | 578 | //Send_Flag == 1 |
| roger5641 | 15:3e0a4079b28a | 579 | Command_Flag = 1; |
| roger5641 | 15:3e0a4079b28a | 580 | Receive_Flag = 0; |
| roger5641 | 15:3e0a4079b28a | 581 | Receive_Counter = 0; |
| roger5641 | 15:3e0a4079b28a | 582 | } |
| roger5641 | 15:3e0a4079b28a | 583 | } |
| roger5641 | 15:3e0a4079b28a | 584 | |
| roger5641 | 15:3e0a4079b28a | 585 | else |
| roger5641 | 15:3e0a4079b28a | 586 | { |
| roger5641 | 15:3e0a4079b28a | 587 | if(Temp == 36) //'$' |
| roger5641 | 15:3e0a4079b28a | 588 | { |
| roger5641 | 15:3e0a4079b28a | 589 | Receive_Flag = 1; |
| roger5641 | 15:3e0a4079b28a | 590 | Receive_Counter = 0; |
| roger5641 | 15:3e0a4079b28a | 591 | Receive_Data[0] = Temp; |
| roger5641 | 15:3e0a4079b28a | 592 | } |
| roger5641 | 15:3e0a4079b28a | 593 | else |
| roger5641 | 15:3e0a4079b28a | 594 | { |
| roger5641 | 15:3e0a4079b28a | 595 | // Waiting |
| roger5641 | 15:3e0a4079b28a | 596 | } |
| roger5641 | 15:3e0a4079b28a | 597 | } |
| roger5641 | 15:3e0a4079b28a | 598 | |
| roger5641 | 15:3e0a4079b28a | 599 | IFS1bits.U2RXIF = 0; |
| roger5641 | 15:3e0a4079b28a | 600 | } |
| roger5641 | 17:d96afd9d2692 | 601 | */ |
| roger5641 | 15:3e0a4079b28a | 602 | |
| smilestone520 | 10:06cc2b1d2aaf | 603 | void init_TIMER(void) |
| smilestone520 | 10:06cc2b1d2aaf | 604 | { |
| smilestone520 | 10:06cc2b1d2aaf | 605 | timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) |
| smilestone520 | 10:06cc2b1d2aaf | 606 | } |
| smilestone520 | 10:06cc2b1d2aaf | 607 | |
| smilestone520 | 10:06cc2b1d2aaf | 608 | void init_PWM(void) |
| smilestone520 | 10:06cc2b1d2aaf | 609 | { |
| smilestone520 | 10:06cc2b1d2aaf | 610 | pwm1.period_us(50); |
| smilestone520 | 10:06cc2b1d2aaf | 611 | pwm1.write(0.5); |
| smilestone520 | 10:06cc2b1d2aaf | 612 | TIM1->CCER |= 0x4; |
| smilestone520 | 10:06cc2b1d2aaf | 613 | |
| smilestone520 | 10:06cc2b1d2aaf | 614 | pwm2.period_us(50); |
| smilestone520 | 10:06cc2b1d2aaf | 615 | pwm2.write(0.5); |
| smilestone520 | 10:06cc2b1d2aaf | 616 | TIM1->CCER |= 0x40; |
| smilestone520 | 10:06cc2b1d2aaf | 617 | } |
| smilestone520 | 10:06cc2b1d2aaf | 618 | |
| smilestone520 | 10:06cc2b1d2aaf | 619 | void init_CN(void) |
| smilestone520 | 10:06cc2b1d2aaf | 620 | { |
| smilestone520 | 10:06cc2b1d2aaf | 621 | HallA_1.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 622 | HallA_1.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 623 | HallB_1.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 624 | HallB_1.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 625 | |
| smilestone520 | 10:06cc2b1d2aaf | 626 | HallA_2.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 627 | HallA_2.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 628 | HallB_2.rise(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 629 | HallB_2.fall(&CN_interrupt); |
| smilestone520 | 10:06cc2b1d2aaf | 630 | |
| smilestone520 | 10:06cc2b1d2aaf | 631 | stateA_1 = HallA_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 632 | stateB_1 = HallB_1.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 633 | stateA_2 = HallA_2.read(); |
| smilestone520 | 10:06cc2b1d2aaf | 634 | stateB_2 = HallB_2.read(); |
| smilestone520 | 20:5b892e37a958 | 635 | } |
| smilestone520 | 20:5b892e37a958 | 636 | |
| smilestone520 | 20:5b892e37a958 | 637 | //**************** funcBorder() ***************************************** |
| smilestone520 | 20:5b892e37a958 | 638 | |
| smilestone520 | 20:5b892e37a958 | 639 | void funcBorder(void) // check if car is too close to the border // miss the ball restriction part |
| smilestone520 | 20:5b892e37a958 | 640 | { |
| smilestone520 | 20:5b892e37a958 | 641 | if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){ |
| smilestone520 | 20:5b892e37a958 | 642 | |
| smilestone520 | 20:5b892e37a958 | 643 | if(yC > yB1) // 靠近上邊界 ***************************** |
| smilestone520 | 20:5b892e37a958 | 644 | { |
| smilestone520 | 20:5b892e37a958 | 645 | if(xC < xB3) //左上角 |
| smilestone520 | 20:5b892e37a958 | 646 | { |
| smilestone520 | 20:5b892e37a958 | 647 | if(angleC <0 && angleC >=-90) // I |
| smilestone520 | 20:5b892e37a958 | 648 | {} |
| smilestone520 | 20:5b892e37a958 | 649 | //clockwise, to angleC = 45 |
| smilestone520 | 20:5b892e37a958 | 650 | |
| smilestone520 | 20:5b892e37a958 | 651 | else if(angleC > -180 && angleC <-90) // II |
| smilestone520 | 20:5b892e37a958 | 652 | {} |
| smilestone520 | 20:5b892e37a958 | 653 | //back 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 654 | |
| smilestone520 | 20:5b892e37a958 | 655 | else if(angleC <=180 && angleC >=90) // III |
| smilestone520 | 20:5b892e37a958 | 656 | {} |
| smilestone520 | 20:5b892e37a958 | 657 | //counter clockwise, to angleC = 45 |
| smilestone520 | 20:5b892e37a958 | 658 | |
| smilestone520 | 20:5b892e37a958 | 659 | else if(angleC < 90 && angleC >=0) // IV |
| smilestone520 | 20:5b892e37a958 | 660 | {} |
| smilestone520 | 20:5b892e37a958 | 661 | //forward 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 662 | |
| smilestone520 | 20:5b892e37a958 | 663 | } |
| smilestone520 | 20:5b892e37a958 | 664 | else if(xC > xB4) //右上角 |
| smilestone520 | 20:5b892e37a958 | 665 | { |
| smilestone520 | 20:5b892e37a958 | 666 | if(angleC <0 && angleC >=-90) // I |
| smilestone520 | 20:5b892e37a958 | 667 | {} |
| smilestone520 | 20:5b892e37a958 | 668 | //back 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 669 | |
| smilestone520 | 20:5b892e37a958 | 670 | else if(angleC > -180 && angleC <-90) // II |
| smilestone520 | 20:5b892e37a958 | 671 | {} |
| smilestone520 | 20:5b892e37a958 | 672 | //counter clockwise, to angleC = 135 |
| smilestone520 | 20:5b892e37a958 | 673 | |
| smilestone520 | 20:5b892e37a958 | 674 | else if(angleC <=180 && angleC >=90) // III |
| smilestone520 | 20:5b892e37a958 | 675 | {} |
| smilestone520 | 20:5b892e37a958 | 676 | //forward 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 677 | |
| smilestone520 | 20:5b892e37a958 | 678 | else if(angleC < 90 && angleC >=0) // IV |
| smilestone520 | 20:5b892e37a958 | 679 | { |
| smilestone520 | 20:5b892e37a958 | 680 | //clockwise, to angleC = 135 |
| smilestone520 | 20:5b892e37a958 | 681 | } |
| smilestone520 | 20:5b892e37a958 | 682 | } |
| smilestone520 | 20:5b892e37a958 | 683 | else //上邊界 |
| smilestone520 | 20:5b892e37a958 | 684 | { |
| smilestone520 | 20:5b892e37a958 | 685 | if(angleC == -90 || angleC == 90) |
| smilestone520 | 20:5b892e37a958 | 686 | {} // do nothing, turn to next point |
| smilestone520 | 20:5b892e37a958 | 687 | else |
| smilestone520 | 20:5b892e37a958 | 688 | {smallAngle(90);} // turn to 90, then turn to next point |
| smilestone520 | 20:5b892e37a958 | 689 | } |
| smilestone520 | 20:5b892e37a958 | 690 | |
| smilestone520 | 20:5b892e37a958 | 691 | |
| smilestone520 | 20:5b892e37a958 | 692 | } |
| smilestone520 | 20:5b892e37a958 | 693 | else if(yC < yB2) // 靠近下邊界 *E****************************************************88 |
| smilestone520 | 20:5b892e37a958 | 694 | { |
| smilestone520 | 20:5b892e37a958 | 695 | if(xC < xB3) //左下角 |
| smilestone520 | 20:5b892e37a958 | 696 | { |
| smilestone520 | 20:5b892e37a958 | 697 | if(angleC <0 && angleC >=-90) // I |
| smilestone520 | 20:5b892e37a958 | 698 | {} |
| smilestone520 | 20:5b892e37a958 | 699 | //forward 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 700 | |
| smilestone520 | 20:5b892e37a958 | 701 | else if(angleC > -180 && angleC <-90) // II |
| smilestone520 | 20:5b892e37a958 | 702 | {} |
| smilestone520 | 20:5b892e37a958 | 703 | //clockwise, to angleC = -45 |
| smilestone520 | 20:5b892e37a958 | 704 | |
| smilestone520 | 20:5b892e37a958 | 705 | else if(angleC <=180 && angleC >=90) // III |
| smilestone520 | 20:5b892e37a958 | 706 | {} |
| smilestone520 | 20:5b892e37a958 | 707 | //back 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 708 | |
| smilestone520 | 20:5b892e37a958 | 709 | else if(angleC < 90 && angleC >=0) // IV |
| smilestone520 | 20:5b892e37a958 | 710 | {} |
| smilestone520 | 20:5b892e37a958 | 711 | //counter clockwise, to angleC = -45 |
| smilestone520 | 20:5b892e37a958 | 712 | |
| smilestone520 | 20:5b892e37a958 | 713 | } |
| smilestone520 | 20:5b892e37a958 | 714 | else if(xC > xB4) //右下角 |
| smilestone520 | 20:5b892e37a958 | 715 | { |
| smilestone520 | 20:5b892e37a958 | 716 | if(angleC <0 && angleC >=-90) // I |
| smilestone520 | 20:5b892e37a958 | 717 | {} |
| smilestone520 | 20:5b892e37a958 | 718 | //counter clockwise, to angleC = -135 |
| smilestone520 | 20:5b892e37a958 | 719 | |
| smilestone520 | 20:5b892e37a958 | 720 | else if(angleC > -180 && angleC <-90) // II |
| smilestone520 | 20:5b892e37a958 | 721 | {} |
| smilestone520 | 20:5b892e37a958 | 722 | //forward 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 723 | |
| smilestone520 | 20:5b892e37a958 | 724 | else if(angleC <=180 && angleC >=90) // III |
| smilestone520 | 20:5b892e37a958 | 725 | {} |
| smilestone520 | 20:5b892e37a958 | 726 | //clockwise, to angleC = -135 |
| smilestone520 | 20:5b892e37a958 | 727 | |
| smilestone520 | 20:5b892e37a958 | 728 | else if(angleC < 90 && angleC >=0) // IV |
| smilestone520 | 20:5b892e37a958 | 729 | {} |
| smilestone520 | 20:5b892e37a958 | 730 | //back 0.5*longC, judge again |
| smilestone520 | 20:5b892e37a958 | 731 | |
| smilestone520 | 20:5b892e37a958 | 732 | } |
| smilestone520 | 20:5b892e37a958 | 733 | else //下邊界 |
| smilestone520 | 20:5b892e37a958 | 734 | { |
| smilestone520 | 20:5b892e37a958 | 735 | if(angleC == -90 || angleC == 90) |
| smilestone520 | 20:5b892e37a958 | 736 | {} // do nothing, turn to next point |
| smilestone520 | 20:5b892e37a958 | 737 | else |
| smilestone520 | 20:5b892e37a958 | 738 | {smallAngle(-90);} // turn to 90, then turn to next point |
| smilestone520 | 20:5b892e37a958 | 739 | } |
| smilestone520 | 20:5b892e37a958 | 740 | |
| smilestone520 | 20:5b892e37a958 | 741 | |
| smilestone520 | 20:5b892e37a958 | 742 | } |
| smilestone520 | 20:5b892e37a958 | 743 | else if(xC < xB3) //靠近左邊界**************************************************************** |
| smilestone520 | 20:5b892e37a958 | 744 | { |
| smilestone520 | 20:5b892e37a958 | 745 | if(angleC == 0 || angleC == 180) |
| smilestone520 | 20:5b892e37a958 | 746 | {} // do nothing, turn to next point |
| smilestone520 | 20:5b892e37a958 | 747 | else |
| smilestone520 | 20:5b892e37a958 | 748 | {smallAngle(0);} // turn to 90, then turn to next point |
| smilestone520 | 20:5b892e37a958 | 749 | } |
| smilestone520 | 20:5b892e37a958 | 750 | |
| smilestone520 | 20:5b892e37a958 | 751 | else if(xC > xB4) //靠近右邊界 *********************************************88 |
| smilestone520 | 20:5b892e37a958 | 752 | { |
| smilestone520 | 20:5b892e37a958 | 753 | if(angleC == 0 || angleC == 180) |
| smilestone520 | 20:5b892e37a958 | 754 | {} // do nothing, turn to next point |
| smilestone520 | 20:5b892e37a958 | 755 | else |
| smilestone520 | 20:5b892e37a958 | 756 | {smallAngle(180);} // turn to 90, then turn to next point |
| smilestone520 | 20:5b892e37a958 | 757 | } |
| smilestone520 | 20:5b892e37a958 | 758 | } |
| smilestone520 | 20:5b892e37a958 | 759 | else |
| smilestone520 | 20:5b892e37a958 | 760 | {aI_State = 2;} |
| smilestone520 | 20:5b892e37a958 | 761 | } // funcBorder() |
| smilestone520 | 20:5b892e37a958 | 762 | //**************** smallAngle() ***************************************** |
| smilestone520 | 20:5b892e37a958 | 763 | |
| smilestone520 | 20:5b892e37a958 | 764 | void smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system |
| smilestone520 | 20:5b892e37a958 | 765 | {} |
| smilestone520 | 20:5b892e37a958 | 766 | if(angleR > 0) // CW |
| smilestone520 | 20:5b892e37a958 | 767 | { |
| smilestone520 | 20:5b892e37a958 | 768 | // turn clockwise to goodAngle; |
| smilestone520 | 20:5b892e37a958 | 769 | } |
| smilestone520 | 20:5b892e37a958 | 770 | else if(angleR < 0) //CCW |
| smilestone520 | 20:5b892e37a958 | 771 | { |
| smilestone520 | 20:5b892e37a958 | 772 | // turn counter clockwise to goodAngle; |
| smilestone520 | 20:5b892e37a958 | 773 | } |
| smilestone520 | 20:5b892e37a958 | 774 | |
| smilestone520 | 20:5b892e37a958 | 775 | |
| smilestone520 | 20:5b892e37a958 | 776 | //*************** turnCW() *********************************** |
| smilestone520 | 20:5b892e37a958 | 777 | |
| smilestone520 | 20:5b892e37a958 | 778 | void turnCW(float goodAngle) |
| smilestone520 | 20:5b892e37a958 | 779 | { |
| smilestone520 | 20:5b892e37a958 | 780 | // turn clockwise to goodAngle; |
| smilestone520 | 20:5b892e37a958 | 781 | } |
| smilestone520 | 20:5b892e37a958 | 782 | |
| smilestone520 | 20:5b892e37a958 | 783 | //*************** turnCCW() *********************************** |
| smilestone520 | 20:5b892e37a958 | 784 | |
| smilestone520 | 20:5b892e37a958 | 785 | void turnCCW(float goodAngle) |
| smilestone520 | 20:5b892e37a958 | 786 | { |
| smilestone520 | 20:5b892e37a958 | 787 | // turn counter clockwise to goodAngle; |
| smilestone520 | 10:06cc2b1d2aaf | 788 | } |