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
Revision 4:ae47210fa346, committed 2016-05-24
- Comitter:
- smilestone520
- Date:
- Tue May 24 07:32:18 2016 +0000
- Parent:
- 3:9f5346fc58b3
- Child:
- 5:794d7d690dc9
- Commit message:
- angle fix
Changed in this revision
| ai.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/ai.cpp Tue May 24 06:43:06 2016 +0000
+++ b/ai.cpp Tue May 24 07:32:18 2016 +0000
@@ -7,7 +7,7 @@
float xP,yP //position that car need to reach
-float angleR // relative angle between car-nextSpot angle and car's angle, angleP - angle C, based on 0~360 degree system
+float angleR // angleR = car-nextSpot direction ---> car direction
**** specs of everything ***************
@@ -21,15 +21,15 @@
***** Code Begin **********
-yB1 yB2 xB3 xB4 // broder conditions
+int yB1 yB2 xB3 xB4; // broder conditions
-yB1 = 0-longC
+yB1 = longC;
-yB2 = 0-wideB + longC
+yB2 = wideB - longC ;
-xB3 = longC
+xB3 = longC ;
-xB4 = longB-longC
+xB4 = longB-longC;
*******************************************************************************
遠離邊界
@@ -57,40 +57,40 @@
{
if(xC < xB3) //左上角
{
- if(angleC <= 90 && angleC >0) // I
+ if(angleC <0 && angleC >=-90) // I
- //clockwise, to angleC = -45
+ //clockwise, to angleC = 45
- else if(angleC <= 180 && angleC >90) // II
+ else if(angleC > -180 && angleC <-90) // II
//back 0.5*longC, judge again
- else if(angleC <= 270 && angleC 180) // III
+ else if(angleC <=180 && angleC >=90) // III
- //counter clockwise, to angleC = -45
+ //counter clockwise, to angleC = 45
- else if(angleC <= 360 && angleC 270) // IV
+ else if(angleC < 90 && angleC >=0) // IV
//forward 0.5*longC, judge again
}
else if(xC > xB4) //右上角
{
- if(angleC <= 90 && angleC >0) // I
+ if(angleC <0 && angleC >=-90) // I
//back 0.5*longC, judge again
- else if(angleC <= 180 && angleC >90) // II
+ else if(angleC > -180 && angleC <-90) // II
- //counter clockwise, to angleC = -135
+ //counter clockwise, to angleC = 135
- else if(angleC <= 270 && angleC 180) // III
+ else if(angleC <=180 && angleC >=90) // III
//forward 0.5*longC, judge again
- else if(angleC <= 360 && angleC 270) // IV
+ else if(angleC < 90 && angleC >=0) // IV
- //clockwise, to angleC = -135
+ //clockwise, to angleC = 135
}
else //上邊界
@@ -98,7 +98,7 @@
if(angleC = -90 || angleC = 90)
{} // do nothing, turn to next point
else
- {smallAngle(-90);} // turn to 90, then turn to next point
+ {smallAngle(90);} // turn to 90, then turn to next point
}
}
@@ -106,38 +106,38 @@
{
if(xC < xB3) //左下角
{
- if(angleC <= 90 && angleC >0) // I
+ if(angleC <0 && angleC >=-90) // I
//forward 0.5*longC, judge again
- else if(angleC <= 180 && angleC >90) // II
+ else if(angleC > -180 && angleC <-90) // II
- //clockwise, to angleC = 45
+ //clockwise, to angleC = -45
- else if(angleC <= 270 && angleC 180) // III
+ else if(angleC <=180 && angleC >=90) // III
//back 0.5*longC, judge again
- else if(angleC <= 360 && angleC 270) // IV
+ else if(angleC < 90 && angleC >=0) // IV
- //counter clockwise, to angleC = 45
+ //counter clockwise, to angleC = -45
}
else if(xC > xB4) //右下角
{
- if(angleC <= 90 && angleC >0) // I
+ if(angleC <0 && angleC >=-90) // I
- //counter clockwise, to angleC = 135
+ //counter clockwise, to angleC = -135
- else if(angleC <= 180 && angleC >90) // II
+ else if(angleC > -180 && angleC <-90) // II
//forward 0.5*longC, judge again
- else if(angleC <= 270 && angleC 180) // III
+ else if(angleC <=180 && angleC >=90) // III
- //clockwise, to angleC = 135
+ //clockwise, to angleC = -135
- else if(angleC <= 360 && angleC 270) // IV
+ else if(angleC < 90 && angleC >=0) // IV
//back 0.5*longC, judge again
@@ -147,7 +147,7 @@
if(angleC = -90 || angleC = 90)
{} // do nothing, turn to next point
else
- {smallAngle(90);} // turn to 90, then turn to next point
+ {smallAngle(-90);} // turn to 90, then turn to next point
}
else if(xC < xB3) //靠近左邊界
@@ -171,11 +171,11 @@
smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
{
- if(angleR - angleC <= 180 ) // CW
+ if(angleR > 0) // CW
{
// turn clockwise to goodAngle;
}
- else if(angleR - angleC > 180) //CCW
+ else if(angleR < 0) //CCW
{
// turn counter clockwise to goodAngle;
}