Robotics Term Project
/
Robottics_Motion
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Diff: AI_Friday.cpp
- Revision:
- 33:587e6dc249b8
- Parent:
- 32:fb8f40fde82c
- Child:
- 34:b0ae940a2fb5
diff -r fb8f40fde82c -r 587e6dc249b8 AI_Friday.cpp
--- a/AI_Friday.cpp Thu May 26 11:41:55 2016 +0000
+++ b/AI_Friday.cpp Thu May 26 16:35:54 2016 +0000
@@ -50,7 +50,7 @@
float v2 = 0.0, v2_ref = 0.0;
float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
-int angle = 90;
+int angle = 0;
float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
int Receive_Data[33] = {0};
@@ -58,27 +58,27 @@
int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
-float k_forward = 20, k_turn = 10;
+float k_forward = 0.3, k_turn = 1;
//**** receive and return by bluetooth ************** // bluetooth.getc()
float xC, yC; // car's position
float angleC;// car's angle
double xP,yP,distance; //position that car need to reach
float angleR; // angleR = car-nextSpot direction ---> car direction
-int xGate = 136, yGate = 328; //左們136 328, 右們874,316
+int xGate = 306, yGate = 124; //左們136 328, 右們874,316
double angleGate, distanceGate;
-int aI_State = 0;
+int aI_State = 2;
int bor_state = 1;
double pi = 3.1415926;
double borAngle=0;
float ballSize; // ball size
-float longC = 65; // car's length //65
-int yB1= 65;
-int yB2 = 580;
-int xB3 = 170;
-int xB4 = 860; // broder conditions
+float longC = 32; // car's length //65
+int yB1 = 70;
+int yB2 = 232;
+int xB3 = 27;
+int xB4 = 305; // broder conditions
//65 , 580, 170,860
int ticCheck = 10;
@@ -238,8 +238,8 @@
case 2: // 前進 再判別 v1+ v2-
if(ticCheck >0) //誤差容忍正負五度
{
- v1_ref = 100;
- v2_ref = -100;
+ v1_ref = 50;
+ v2_ref = -50;
ticCheck -- ;
}
else
@@ -252,8 +252,8 @@
if(ticCheck >0) //誤差容忍正負五度
{
- v1_ref = -100;
- v2_ref = +100;
+ v1_ref = -50;
+ v2_ref = 50;
ticCheck -- ;
}
else
@@ -266,8 +266,8 @@
if((borAngle-angleC)> 5 || (borAngle-angleC)<-5)//誤差容忍正負五度
{
- v1_ref = -100;
- v2_ref = -100;
+ v1_ref = -50;
+ v2_ref = -50;
}
else
{
@@ -279,8 +279,8 @@
case 5: // 逆時轉到特定角 結束 v1 ++ v2++
if((borAngle-angleC)> 5 || (borAngle-angleC)<-5)//誤差容忍正負五度
{
- v1_ref = 100;
- v2_ref = 100;
+ v1_ref = 50;
+ v2_ref = 50;
}
else
{
@@ -295,92 +295,18 @@
//****to case2
break;
case 2://move to get ball
- if(-3<=angleR<=3) // direct to the ball
- {
- if(distance<=10) // check夾具距離
- {
- v1_ref = 0;
- v2_ref = 0;
- aI_State = 3;
- }
- else if(10<distance<50)
- {
- v1_ref = 100;
- v2_ref = -100;
- }
- else
- {
- v1_ref = 300;
- v2_ref = -300;
- }
- }
-
- else if(3<angleR<=15) // small angle right
- {
- if(distance<50)
- {
- v1_ref = 200;
- v2_ref = -(v1_ref-angleR*k_turn);
- }
- else
- {
- v1_ref = 300;
- v2_ref = -(v1_ref-angleR*k_turn);
- }
-
- }
- else if(-15 <= angleR < -3) // small angle left
- {
- if(distance<50)
- {
- v1_ref = -(v2_ref+angleR*k_turn);
- v2_ref = 200;
- }
- else
- {
- v1_ref = -(v2_ref+angleR*k_turn);
- v2_ref = 300;
- }
-
- }
- else if(15<angleR<=180) // big angle right
- {
- v1_ref = 200;
- v2_ref = 200;
- wait(10); //need to count the relation between angle and time in v = 200
- if(distance<50)
- {
- v1_ref = 100;
- v2_ref = -100;
- }
- else
- {
- v1_ref = 300;
- v2_ref = -300;
- }
-
- }
- else if(-180<=angleR<-15) // big angle left
- {
- v1_ref = -200;
- v2_ref = -200;
- wait(10); //need to count the relation between angle and time in v = 200
- if(0<distance<50)
- {
- v1_ref = 100;
- v2_ref = -100;
- }
- else
- {
- v1_ref = 300;
- v2_ref = -300;
- }
-
- }
-
+ if((distance == 50) && (-5<=angleR<=5))
+ {
+ aI_State = 3;
+ }
+ else
+ {
+ v1_ref = k_forward*(distance - 50) + k_turn*angleR;
+ v2_ref = -k_forward*(distance - 50) - k_turn*angleR;
+ }
break;
case 3: //****getBall();
- if((distance == 10) && (-3<=angleR<=3))
+ if((distance == 50) && (-5<=angleR<=5))
{
angle = -10;
servo_duty = 0.079 + (0.084/180)*angle;
@@ -400,42 +326,42 @@
angleGate = atan2((yGate-yP),(xGate-xP))*180/pi;
if(-3<=angleGate<=3) // direct to the gate
{
- if(distanceGate<=10) // check distance between car cemtroid and gate
+ if(distanceGate<=80) // check distance between car cemtroid and gate
{
v1_ref = 0;
v2_ref = 0;
aI_State = 5;
}
- else if(10<distanceGate<50)
+ else if(80<distanceGate<150)
{
- v1_ref = 100;
- v2_ref = -100;
+ v1_ref = 50;
+ v2_ref = -50;
}
else
{
- v1_ref = 300;
- v2_ref = -300;
+ v1_ref = 50;
+ v2_ref = -50;
}
}
else if(3<angleGate<=15) // small angle right
{
if(distanceGate<50)
{
- v1_ref = 200;
+ v1_ref = 50;
v2_ref = -(v1_ref-angleGate*k_turn);
}
else
{
- v1_ref = 300;
+ v1_ref = 50;
v2_ref = -(v1_ref-angleGate*k_turn);
}
}
else if(-15<=angleGate<-3) // small angle left
{
- if(distanceGate<50)
+ if(distanceGate<150)
{
v1_ref = -(v2_ref+angleGate*k_turn);
- v2_ref = 200;
+ v2_ref = 50;
}
else
{
@@ -445,34 +371,34 @@
}
else if(15<angleGate<=180) // big angle right
{
- v1_ref = 200;
- v2_ref = 200;
- wait(10); //need to count the relation between angle and time in v = 200
- if(distanceGate<50)
+ v1_ref = 50;
+ v2_ref = 50;
+ wait(0.005); //need to count the relation between angle and time in v = 200
+ if(0<=distanceGate<150)
{
- v1_ref = 100;
- v2_ref = -100;
+ v1_ref = 50;
+ v2_ref = -50;
}
else
{
- v1_ref = 300;
- v2_ref = -300;
+ v1_ref = 50;
+ v2_ref = -50;
}
}
else if(-180<=angleGate<-15) // big angle left
{
- v1_ref = -200;
- v2_ref = -200;
- wait(10); //need to count the relation between angle and time in v = 200
- if(0<distanceGate<50)
+ v1_ref = -50;
+ v2_ref = -50;
+ wait(0.005); //need to count the relation between angle and time in v = 200
+ if(0<distanceGate<150)
{
- v1_ref = 100;
- v2_ref = -100;
+ v1_ref = 50;
+ v2_ref = -50;
}
else
{
- v1_ref = 300;
- v2_ref = -300;
+ v1_ref = 50;
+ v2_ref = -50;
}
}
@@ -484,23 +410,23 @@
servo_duty = 0.079 + (0.084/180)*angle;
servo.write(servo_duty);
servo = 1;
- wait(0.1);
+ //wait(0.1);
servo = 0;
pwm1.write(0.1f + 0.5f); // push
pwm2.write(-0.1f + 0.5f);
- wait(0.5);
+ //wait(0.5);
angle = -10; // Fixture down
servo_duty = 0.079 + (0.084/180)*angle;
servo.write(servo_duty);
servo = 1;
- wait(0.1);
+ //wait(0.1);
servo = 0;
pwm1.write(-0.3f + 0.5f); // back and leave gate
pwm2.write(0.3f + 0.5f);
- wait(1);
+ //wait(1);
aI_State = 0;
@@ -514,7 +440,7 @@
void CN_interrupt(void)
{
- //Motor 1
+ /*//Motor 1
stateA_1 = HallA_1.read();
stateB_1 = HallB_1.read();
@@ -609,7 +535,7 @@
else if(state_2-state_2_old == 3)
v2Count=v2Count-1;
}
- state_2_old = state_2;
+ state_2_old = state_2;*/
//////////////////////////////////
@@ -617,6 +543,8 @@
//v2Count +1
//Inverse
//v2Count -1
+
+
}
void init_TIMER(void)