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
ai.cpp
- Committer:
- smilestone520
- Date:
- 2016-05-24
- Revision:
- 2:4988d0424f29
- Parent:
- 1:a6e23913e047
- Child:
- 3:9f5346fc58b3
File content as of revision 2:4988d0424f29:
**** receive and return by bluetooth **************
float xC, yC // car's position
float angleC // car's angle
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
**** specs of everything ***************
float longC // car's length
float ballSize // ball size
float longB // long of the court
float wideB // wide of the court
***** Code Begin **********
yB1 yB2 xB3 xB4 // broder conditions
yB1 = 0-longC
yB2 = 0-wideB + longC
xB3 = longC
xB4 = longB-longC
*******************************************************************************
遠離邊界
car:
matlab:
********************************************************************************
**************** funcBorder() *****************************************
funcBorder() // check if car is too close to the border
if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
if(yC > yB1) // 靠近上邊界
{
if(xC < xB3) //左上角
{
if(angleC <= 90 && angleC >0) // I
//clockwise, to angleC = -45
else if(angleC <= 180 && angleC >90) // II
//back 0.5*longC, judge again
else if(angleC <= 270 && angleC 180) // III
//counter clockwise, to angleC = -45
else if(angleC <= 360 && angleC 270) // IV
//forward 0.5*longC, judge again
}
else if(xC > xB4) //右上角
{
if(angleC <= 90 && angleC >0) // I
//back 0.5*longC, judge again
else if(angleC <= 180 && angleC >90) // II
//counter clockwise, to angleC = -135
else if(angleC <= 270 && angleC 180) // III
//forward 0.5*longC, judge again
else if(angleC <= 360 && angleC 270) // IV
//clockwise, to angleC = -135
}
else //上邊界
{
if(angleC = -90 || angleC = 90)
{} // do nothing, turn to next point
else
{smallAngle(-90);} // turn to 90, then turn to next point
}
}
else if(yC < yB2) // 靠近下邊界
{
if(xC < xB3) //左下角
{
if(angleC <= 90 && angleC >0) // I
//forward 0.5*longC, judge again
else if(angleC <= 180 && angleC >90) // II
//clockwise, to angleC = 45
else if(angleC <= 270 && angleC 180) // III
//back 0.5*longC, judge again
else if(angleC <= 360 && angleC 270) // IV
//counter clockwise, to angleC = 45
}
else if(xC > xB4) //右下角
{
if(angleC <= 90 && angleC >0) // I
//counter clockwise, to angleC = 135
else if(angleC <= 180 && angleC >90) // II
//forward 0.5*longC, judge again
else if(angleC <= 270 && angleC 180) // III
//clockwise, to angleC = 135
else if(angleC <= 360 && angleC 270) // IV
//back 0.5*longC, judge again
}
else //下邊界
{
if(angleC = -90 || angleC = 90)
{} // do nothing, turn to next point
else
{smallAngle(90);} // turn to 90, then turn to next point
}
else if(xC < xB3) //靠近左邊界
{
if(angleC = 0 || angleC = 180)
{} // do nothing, turn to next point
else
{smallAngle(0);} // turn to 90, then turn to next point
}
else if(xC > xB4) //靠近右邊界
{
if(angleC = 0 || angleC = 180)
{} // do nothing, turn to next point
else
{smallAngle(180);} // turn to 90, then turn to next point
}
}
**************** smallAngle() *****************************************
smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
{
if(angleR - angleC <= 180 ) // CW
{
// turn clockwise to goodAngle;
}
else if(angleR - angleC > 180) //CCW
{
// turn counter clockwise to goodAngle;
}
}
*************** turnCW() ***********************************
turnCW(float goodAngle)
{
// turn clockwise to goodAngle;
}
*************** turnCCW() ***********************************
turnCCW(float goodAngle)
{
// turn counter clockwise to goodAngle;
}
**********