Robotics Term Project / Mbed 2 deprecated Robottics_Motion

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

AI_Friday.cpp@25:7053736ea6cc, 2016-05-26 (annotated)

Committer:
smilestone520
Date:
Thu May 26 05:08:24 2016 +0000
Revision:
25:7053736ea6cc
Parent:
24:f61847968e72
Child:
26:dbdbfdb4dd41
hahah;

Who changed what in which revision?

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