Revision 19:3ca10fe26131, committed 2015-10-12

Comitter:

ewoud

Date:

Mon Oct 12 08:13:29 2015 +0000

Parent:

18:4ee32b922251

Commit message:

last version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_VelocityExample.lib	Mon Oct 12 08:13:29 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/Numero-Uno/code/PID_VelocityExample/#4ee32b922251

--- a/inits.h	Wed Oct 07 13:19:25 2015 +0000
+++ b/inits.h	Mon Oct 12 08:13:29 2015 +0000
@@ -2,7 +2,7 @@
 // Defines
 //****************************************************************************/
 #define RATE  0.01 
-#define calcRATE 0.5
+#define calcRATE 0.2
 #define Kc    1.5
 #define Ti    0.8
 #define Td    0.0
@@ -33,7 +33,7 @@
 // EMG input
 AnalogIn pot1(A0);
 AnalogIn pot2(A1);
-
+float maxspeed=10; // xy in cm/s
 
 // Timers
 Ticker motorControlTicker;

--- a/main.cpp	Wed Oct 07 13:19:25 2015 +0000
+++ b/main.cpp	Mon Oct 12 08:13:29 2015 +0000
@@ -52,7 +52,9 @@
             else {
                 request = -request_neg;
             }
-                        
+            
+            request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
+                       
             // calculate required rotational velocity from the requested horizontal velocity
             // first get the current position from the motor encoders
             // make them start at 45 degree.
@@ -79,21 +81,22 @@
                     //return 0;
                 //}
                 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
-                toX=currentX+request*calcRATE; // should be request*RATE
+                toX=currentX+request/200*calcRATE; // should be request*RATE, 200 is a magical number to make request work in cm/s
+                //toY=currentY+0*calcRATE;
                 toY=currentY+0*calcRATE;
-            
+                
                 toLeftAngle = atan(toY/toX)*180/M_PI;
                 toRightAngle = atan(toY/(l-toX))*180/M_PI;
                 
                 // calculate how much the angles should change in this timestep
-                leftDeltaAngle=(toLeftAngle-leftAngle)/10;
-                rightDeltaAngle=(toRightAngle-rightAngle)/10;
+                leftDeltaAngle=(toLeftAngle-leftAngle);
+                rightDeltaAngle=(toRightAngle-rightAngle);
                 
                 // calculate the neccesairy velocities to make these angles happen.
                 leftRequest=(leftDeltaAngle/calcRATE);
                 rightRequest=(rightDeltaAngle/calcRATE);
             }
-            //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
+            //pc.printf("leftRequest: %f, rightRequest %f, curX: %f, curY: %f \n\r",leftRequest,rightRequest,currentX,currentY);
             calcFlag=false;
         }
         else if (systemOn == true && goFlag == true){