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