Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Revision 14:24f994dc2770, committed 2012-04-28
- Comitter:
- narshu
- Date:
- Sat Apr 28 20:44:09 2012 +0000
- Parent:
- 13:57ea4e520dbd
- Child:
- 15:acae5c0e9ca8
- Commit message:
- added speed limiter
Changed in this revision
--- a/Eurobot_shared/Motion/motion.cpp Sat Apr 28 19:39:08 2012 +0000 +++ b/Eurobot_shared/Motion/motion.cpp Sat Apr 28 20:44:09 2012 +0000 @@ -62,6 +62,7 @@ */ // check if target reached ---------------------------------- + int localtargetnumber = ai.targetnumber; if ( ( abs(currX - ai.target.x) < POSITION_TOR ) &&( abs(currY - ai.target.y) < POSITION_TOR ) ) { @@ -75,8 +76,8 @@ if (abs(diffDir) < ANGLE_TOR) { - static unsigned int lasttargetnumber = 0; - if (ai.targetnumber > lasttargetnumber) { + static unsigned int lasttargetnumber = 1; + if (localtargetnumber > lasttargetnumber) { ai.thr_AI.signal_set(0x01); lasttargetnumber++; } @@ -109,18 +110,33 @@ } else { float tempPidVar = PIDControllerMotorTheta2.compute(); + float MoveSpeedLimiter = 1; //pc.printf("turn,%f\n",diffDir); + + float distanceToX = (float)abs(currX - ai.target.x); + float distanceToY = (float)abs(currY - ai.target.y); + + float distanceToTarget = sqrt(distanceToX*distanceToX+distanceToY*distanceToY); + + if (distanceToTarget < 500) { + MoveSpeedLimiter = (distanceToTarget)/500; + } + // calculte the motor speeds if (tempPidVar >= 0) { //turn left - speedL = (1-abs(tempPidVar))*MOVE_SPEED; - speedR = MOVE_SPEED; + speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter; + speedR = MOVE_SPEED*MoveSpeedLimiter; } else { //turn right - speedR = (1-abs(tempPidVar))*MOVE_SPEED; - speedL = MOVE_SPEED; + speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter; + speedL = MOVE_SPEED*MoveSpeedLimiter; } + + + + if (ai.target.facing) motors.setSpeed( int(speedL), int(speedR)); else motors.setSpeed( -int(speedR), -int(speedL));
--- a/Eurobot_shared/ai/ai.cpp Sat Apr 28 19:39:08 2012 +0000 +++ b/Eurobot_shared/ai/ai.cpp Sat Apr 28 20:44:09 2012 +0000 @@ -12,11 +12,11 @@ void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing) { targetlock.lock(); + targetnumber++; target.x = targetX; target.y = targetY; target.theta = targetTheta; target.facing = targetfacing; - targetnumber++; targetlock.unlock(); }
--- a/Eurobot_shared/ui/ui.cpp Sat Apr 28 19:39:08 2012 +0000 +++ b/Eurobot_shared/ui/ui.cpp Sat Apr 28 20:44:09 2012 +0000 @@ -61,8 +61,11 @@ void UI::printloop() { - //Thread::wait(1500); +#ifdef UION + Thread::wait(1500); +#else Thread::wait(osWaitForever); +#endif char* sync = "ABCD"; std::cout.write(sync, 4);
--- a/globals.h Sat Apr 28 19:39:08 2012 +0000 +++ b/globals.h Sat Apr 28 20:44:09 2012 +0000 @@ -4,6 +4,9 @@ #include "mbed.h" #define PI 3.14159265 +//enables ui +#define UION + //#define ROBOT_SECONDARY #ifdef ROBOT_SECONDARY @@ -28,8 +31,8 @@ //Robot movement constants -const float fwdvarperunit = 0.008; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN! -const float varperang = 0.005; //around 1 degree stddev per 180 turn +const float fwdvarperunit = 0.01; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN! +const float varperang = 0.01; //around 1 degree stddev per 180 turn const float xyvarpertime = 0;//0.0005; //(very poorly) accounts for hitting things const float angvarpertime = 0;//0.001;