Diff: main.cpp

Revision:

2:caf73a1d7827

Parent:

1:a5447cf54fba

Child:

3:6fa07ceb897f

--- a/main.cpp	Mon Oct 22 06:04:06 2012 +0000
+++ b/main.cpp	Mon Oct 22 22:06:11 2012 +0000
@@ -1,29 +1,63 @@
 #include "mbed.h"
 #include "RobotLeg.h"
 #include "Terminal.h"
+#include "CircularBuffer.h"
 #include <cstring>
 
-
+CircularBuffer<float,16> dataLog;
 
 RobotLeg leg(p17, p15, p13);
 
+float radius, speed;
+
 
 
-CmdHandler* move(Terminal* terminal, const char* input)
+CmdHandler* log(Terminal* terminal, const char* input)
 {
-    float x, y, z;
-    bool success = false;
+    int start = 0;
+    int end = 15;
     char output[256];
     
-    if (sscanf(input, "move %f %f %f", &x, &y, &z) == 3)
+    if (sscanf(input, "log %d %d", &start, &end) == 1)
+    {
+        // Print only one item
+        sprintf(output, "%4d: %f\n", start, dataLog[start]);
+        terminal->write(output);
+    }
+    else
     {
-        success = leg.move(x, y, z);
-    }
+        // Print a range of items
+        for (int i = start; i <= end; i++)
+        {
+            sprintf(output, "%4d: %f\n", i, dataLog[i]);
+            terminal->write(output);
+        }
+    }  
     
-    if (success)
-        sprintf(output, "new angles: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
+    return NULL;
+}
+
+
+
+CmdHandler* setspeed(Terminal* terminal, const char* input)
+{
+    const float maxSpeed = 0.5f;
+    const float minSpeed = -0.5f;
+
+    float newSpeed;
+    char output[256];
+    
+    if (sscanf(input, "speed %f", &newSpeed) == 1)
+    {
+        newSpeed = (newSpeed > maxSpeed ? maxSpeed : (newSpeed < minSpeed ? minSpeed : newSpeed));
+        sprintf(output, "new speed: %f m/s", newSpeed);
+        speed = newSpeed;
+    }
     else
-        sprintf(output, "invalid position: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
+    {
+        sprintf(output, "error reading input parameters");
+    }
+
     terminal->write(output);
     
     return NULL;
@@ -31,15 +65,15 @@
 
 
 
-CmdHandler* step(Terminal* terminal, const char* input)
+CmdHandler* setradius(Terminal* terminal, const char* input)
 {
-    float x, y, z;
+    float newRadius;
     char output[256];
     
-    if (sscanf(input, "step %f %f %f", &x, &y, &z) == 3)
+    if (sscanf(input, "radius %f", &newRadius) == 1)
     {
-        leg.step(x, y, z);
-        sprintf(output, "stepping to new position");
+        sprintf(output, "new turning radius: %f m", newRadius);
+        radius = newRadius;
     }
     else
     {
@@ -55,8 +89,16 @@
 
 int main()
 {
+    Timer timer;
+    float angle, arc; // radius, speed are globals now
+    vector3 v;
+    matrix4 P, Q, T; 
     Terminal terminal;
-
+    
+    terminal.addCommand("log", &log);
+    terminal.addCommand("speed", &setspeed);
+    terminal.addCommand("radius", &setradius);
+    
     // Set leg parameters
     leg.setDimensions(0.094f, 0.104f, -0.006f, 0.020f);
     leg.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
@@ -64,11 +106,51 @@
     leg.phi.calibrate(975, 2250, 80.0f, -60.0f);
     leg.psi.calibrate(2400, 1125, 60.0f, -60.0f);
     
-    terminal.addCommand("move", &move);
-    terminal.addCommand("step", &step);
+    // Create matrices to change base from robot coordinates to leg coordinates
+    Q.translate(vector3(0.08f, 0.08f, 0.0f));
+    P = Q.inverse();
+
+    // Go to initial position
+    leg.step(vector3(0.1f, 0.1f, -0.05f));
+    
+    // Set radius and speed to go straight forward
+    radius = 0.0f; // meters
+    speed = 0.0f; // m/s
+    
+    timer.start();
+    
+    /*
+    // Dump debug info 
+    sprintf(output, "T =\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t0\t\t0\t\t0\t\t1\n",
+            T.a11, T.a12, T.a13, T.a14,
+            T.a21, T.a22, T.a23, T.a24,
+            T.a31, T.a32, T.a33, T.a34);
+    terminal.write(output); */
+    
     
     while(true)
     {
-        leg.update();
+        arc = speed*timer.read();
+        dataLog.push(timer.read());
+        timer.reset();
+        
+        if (radius == 0)
+        {
+            angle = 0.0f;
+            v.x = 0.0f;
+            v.y = arc;
+            v.z = 0.0f;
+        }
+        else
+        {
+            angle = -arc/radius;
+            v.x = radius*(1 - cos(-angle));
+            v.y = radius*sin(-angle);
+            v.z = 0.0f;
+        }
+        
+        T.identity().rotateZ(angle).translate(v);
+    
+        leg.update(P*T.inverse()*Q);
     }
 }