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:
- 23:948ce071cbb6
- Parent:
- 22:d7b98234291c
- Child:
- 24:f61847968e72
--- a/AI_Friday.cpp Thu May 26 00:33:51 2016 +0000
+++ b/AI_Friday.cpp Thu May 26 04:49:23 2016 +0000
@@ -260,12 +260,16 @@
v1_ref = -100;
v2_ref = -100;
}
-
+ borAngle = 0;
+ aI_State = 2;
+ bor_state = 1;
break;
case 5: // 逆時轉到特定角 結束 v1 ++ v2++
-
- break;
- case 6: //結束
+ if((borAngle-angleC)> 5 || (borAngle-angleC)<-5)
+ {
+ v1_ref = 100;
+ v2_ref = 100;
+ }
borAngle = 0;
aI_State = 2;
bor_state = 1;
@@ -685,45 +689,49 @@
if(xC < xB3) //左上角
{
if(angleC <0 && angleC >=-90) // I
- {}
- //clockwise, to angleC = 45
+ {
+ bor_state = 4; //clockwise, to angleC = 45
+ borAngle = 45;
+ }
else if(angleC > -180 && angleC <-90) // II
- {}
- //back 0.5*longC, judge again
+ bor_state = 3; //back 0.5*longC, judge again
else if(angleC <=180 && angleC >=90) // III
- {}
- //counter clockwise, to angleC = 45
+ {
+ bor_state = 5;//counter clockwise, to angleC = 45
+ borAngle = 45;
+ }
else if(angleC < 90 && angleC >=0) // IV
- {}
- //forward 0.5*longC, judge again
+ bor_state = 2;//forward 0.5*longC, judge again
}
else if(xC > xB4) //右上角
{
if(angleC <0 && angleC >=-90) // I
- {}
- //back 0.5*longC, judge again
+ bor_state = 3;//back 0.5*longC, judge again
else if(angleC > -180 && angleC <-90) // II
- {}
- //counter clockwise, to angleC = 135
+ {
+ bor_state = 5;//counter clockwise, to angleC = 135
+ borAngle = 135;
+ }
else if(angleC <=180 && angleC >=90) // III
- {}
- //forward 0.5*longC, judge again
+ bor_state = 2;//forward 0.5*longC, judge again
else if(angleC < 90 && angleC >=0) // IV
- {
- //clockwise, to angleC = 135
- }
+ {
+ bor_state = 4;//clockwise, to angleC = 135
+ borAngle = 135;
+ }
+
}
else //上邊界
{
if(angleC == -90 || angleC == 90)
- {} // do nothing, turn to next point
+ {aI_State = 2;} // do nothing, turn to next point
else
{smallAngle(90);} // turn to 90, then turn to next point
}
@@ -735,45 +743,49 @@
if(xC < xB3) //左下角
{
if(angleC <0 && angleC >=-90) // I
- {}
- //forward 0.5*longC, judge again
-
+ bor_state = 2; //forward 0.5*longC, judge again
+
else if(angleC > -180 && angleC <-90) // II
- {}
- //clockwise, to angleC = -45
+ {
+ bor_state = 4; //clockwise, to angleC = -45
+ borAngle = -45;
+ }
else if(angleC <=180 && angleC >=90) // III
- {}
- //back 0.5*longC, judge again
+ bor_state = 3;//back 0.5*longC, judge again
else if(angleC < 90 && angleC >=0) // IV
- {}
- //counter clockwise, to angleC = -45
+ {
+ bor_state = 5;//counter clockwise, to angleC = -45
+ borAngle = -45;
+ }
}
else if(xC > xB4) //右下角
{
if(angleC <0 && angleC >=-90) // I
- {}
- //counter clockwise, to angleC = -135
+ {
+ bor_state = 5;//counter clockwise, to angleC = -135
+ borAngle = -135;
+ }
else if(angleC > -180 && angleC <-90) // II
- {}
- //forward 0.5*longC, judge again
+ bor_state = 2;//forward 0.5*longC, judge again
else if(angleC <=180 && angleC >=90) // III
- {}
- //clockwise, to angleC = -135
+ {
+ bor_state = 4;//clockwise, to angleC = -135
+ borAngle = -135;
+ }
else if(angleC < 90 && angleC >=0) // IV
- {}
- //back 0.5*longC, judge again
+ bor_state = 3;//back 0.5*longC, judge again
}
else //下邊界
{
if(angleC == -90 || angleC == 90)
- {} // do nothing, turn to next point
+ aI_State = 2;} // do nothing, turn to next point
else
{smallAngle(-90);} // turn to 90, then turn to next point
}
@@ -783,14 +795,32 @@
else if(xC < xB3) //靠近左邊界****************************************************************
{
if(angleC == 0 || angleC == 180)
- {} // do nothing, turn to next point
+ aI_State = 2; // do nothing, turn to next point
else
- {smallAngle(0);} // turn to 90, then turn to next point
+ {
+ if(0<=angleC<90 || -90<=angleC<0)
+ {
+ bor_state = 5;
+ borAngle = 90;
+ }
+ } // turn to 90, then turn to next point
}
else if(xC > xB4) //靠近右邊界 *********************************************88
{
if(angleC == 0 || angleC == 180)
+ aI_State = 2; // do nothing, turn to next point
+ else
+ {smallAngle(180);} // turn to 90, then turn to next point
+ }
+ }
+ else
+ {aI_State = 2;}
+} // funcBorder()
+
+ else if(xC > xB4) //靠近右邊界 *********************************************88
+ {
+ if(angleC == 0 || angleC == 180)
{} // do nothing, turn to next point
else
{smallAngle(180);} // turn to 90, then turn to next point