Diff: Eurobot_shared/Motion/motion.cpp

Revision:

14:24f994dc2770

Parent:

13:57ea4e520dbd

Child:

15:acae5c0e9ca8

--- 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));