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;