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@33:587e6dc249b8, 2016-05-26 (annotated)

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