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@29:47519f2b14ad, 2016-05-26 (annotated)

Committer:
smilestone520
Date:
Thu May 26 11:27:25 2016 +0000
Revision:
29:47519f2b14ad
Parent:
28:d576b1190610
Child:
30:2ce51e8719ff
hahaha

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