Diff: main.cpp

Revision:

5:475f67175510

Parent:

3:6fa07ceb897f

Child:

6:0163f2737cc6

diff -r 2dd4c3338895 -r 475f67175510 main.cpp
--- a/main.cpp	Wed Nov 14 04:29:55 2012 +0000
+++ b/main.cpp	Sun Jan 13 02:59:48 2013 +0000
@@ -1,124 +1,92 @@
 #include "mbed.h"
 #include "RobotLeg.h"
 #include "Matrix.h"
-#include "Terminal.h"
 #include "CircularBuffer.h"
+#include "Radio.h"
+#include "commands.h"
 #include <cstring>
 
 
 
 CircularBuffer<float,16> dataLog;
-RobotLeg leg(p17, p15, p13);
 float radius, speed;
-
-
-
-CmdHandler* log(Terminal* terminal, const char* input)
-{
-    int start = 0;
-    int end = 15;
-    char output[256];
-    
-    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
-    {
-        // Print a range of items
-        for (int i = start; i <= end; i++)
-        {
-            sprintf(output, "%4d: %f\n", i, dataLog[i]);
-            terminal->write(output);
-        }
-    }  
-    
-    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, "error reading input parameters");
-    }
-
-    terminal->write(output);
-    
-    return NULL;
-}
-
-
-
-CmdHandler* setradius(Terminal* terminal, const char* input)
-{
-    float newRadius;
-    char output[256];
-    
-    if (sscanf(input, "radius %f", &newRadius) == 1)
-    {
-        sprintf(output, "new turning radius: %f m", newRadius);
-        radius = newRadius;
-    }
-    else
-    {
-        sprintf(output, "error reading input parameters");
-    }
-
-    terminal->write(output);
-    
-    return NULL;
-}
+RobotLeg legA(p26, p29, p30);
+RobotLeg legB(p13, p14, p15);
+RobotLeg legC(p12, p11, p8);
+RobotLeg legD(p23, p24, p25);
+Radio radio(p5, p6, p7, p17, p19, p20);
 
 
 
 int main()
 {
-    Timer timer;
-    float angle, arc; // radius, speed are globals now
+    Timer deltaTimer;
+    Timer cycleTimer;
+    float angle, arc;
     vector3 v;
-    matrix4 P, Q, T; 
+    matrix4 T;
+    matrix4 PA, QA;
+    matrix4 PB, QB; 
+    matrix4 PC, QC; 
+    matrix4 PD, QD; 
     Terminal terminal;
     
     terminal.addCommand("log", &log);
     terminal.addCommand("speed", &setspeed);
     terminal.addCommand("radius", &setradius);
+    terminal.addCommand("read", &read);
     
     // Set leg parameters
-    leg.setDimensions(0.094f, 0.104f, -0.006f, 0.020f);
-    leg.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
-    leg.theta.calibrate(950, 2200, 60.0f, -60.0f);
-    leg.phi.calibrate(975, 2250, 80.0f, -60.0f);
-    leg.psi.calibrate(2400, 1125, 60.0f, -60.0f);
-    
+    legA.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
+    legB.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
+    legC.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
+    legD.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
+    legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
+    legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
+    legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
+    legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
+    legA.setStepCircle(0.12, 0.11, -0.15, 0.11);
+    legB.setStepCircle(0.12, 0.11, -0.15, 0.11);
+    legC.setStepCircle(0.12, 0.11, -0.15, 0.11);
+    legD.setStepCircle(0.12, 0.11, -0.15, 0.11);
+    legA.theta.calibrate(1000, 2000, 45.0f, -45.0f);
+    legA.phi.calibrate(1000, 2000, 70.0f, -45.0f);
+    legA.psi.calibrate(2000, 1000, 80.0f, -45.0f);
+    legB.theta.calibrate(1000, 2000, 45.0f, -45.0f);
+    legB.phi.calibrate(1000, 2000, 70.0f, -45.0f);
+    legB.psi.calibrate(2000, 1000, 80.0f, -45.0f);
+    legC.theta.calibrate(2000, 1000, 45.0f, -45.0f);
+    legC.phi.calibrate(2000, 1000, 70.0f, -45.0f);
+    legC.psi.calibrate(1000, 2000, 80.0f, -45.0f);
+    legD.theta.calibrate(2000, 1000, 45.0f, -45.0f);
+    legD.phi.calibrate(2000, 1000, 70.0f, -45.0f);
+    legD.psi.calibrate(1000, 2000, 80.0f, -45.0f);
+
     // Create matrices to change base from robot coordinates to leg coordinates
-    Q.translate(vector3(0.08f, 0.08f, 0.0f));
-    P = Q.inverse();
+    QA.translate(vector3(0.1f, 0.1f, 0.0f));
+    PA = QA.inverse();
+    QB.translate(vector3(-0.1f, -0.1f, 0.0f));
+    QB.a11 = -1.0f; QB.a22 = -1.0f;
+    PB = QB.inverse();
+    QC.translate(vector3(0.1f, -0.1f, 0.0f));
+    QC.a11 = -1.0f;
+    PC = QC.inverse();
+    QD.translate(vector3(-0.1f, 0.1f, 0.0f));
+    QD.a22 = -1.0f;
+    PD = QD.inverse();
 
     // Go to initial position
-    leg.step(vector3(0.1f, 0.1f, -0.05f));
+    legA.reset(-0.5f);
+    legB.reset(0.0f);
+    legC.reset(0.5f);
+    legD.reset(1.0f);
     
     // Set radius and speed to go straight forward
     radius = 0.0f; // meters
     speed = 0.0f; // m/s
     
-    timer.start();
+    deltaTimer.start();
+    cycleTimer.start();
     
     /*
     // Dump debug info 
@@ -131,9 +99,10 @@
     
     while(true)
     {
-        arc = speed*timer.read();
-        dataLog.push(timer.read());
-        timer.reset();
+        // Get arc length of movement delta
+        arc = speed*deltaTimer.read();
+        dataLog.push(deltaTimer.read());
+        deltaTimer.reset();
         
         if (radius == 0)
         {
@@ -150,8 +119,12 @@
             v.z = 0.0f;
         }
         
-        T.identity().rotateZ(angle).translate(v);
+        // Compute movement transformation in robot coordinates
+        T.identity().rotateZ(angle).translate(v).inverse();
     
-        leg.update(P*T.inverse()*Q);
+        legA.update(PA*T*QA);
+        legB.update(PB*T*QB);
+        legC.update(PC*T*QC);
+        legD.update(PD*T*QD);
     }
 }