Diff: main.cpp

Revision:

12:a952bd74d363

Parent:

11:9ee0214bd410

Child:

13:1c5d255835ce

diff -r 9ee0214bd410 -r a952bd74d363 main.cpp
--- a/main.cpp	Wed May 15 17:22:22 2013 +0000
+++ b/main.cpp	Tue May 21 04:27:58 2013 +0000
@@ -30,6 +30,22 @@
 RobotLeg legC(p19, p11, p8, false);
 RobotLeg legD(p25, p24, p23, false);
 
+CmdHandler* legpos(Terminal* terminal, const char*)
+{
+    char output[256];
+    char abuf[64];
+    char bbuf[64];
+    char cbuf[64];
+    char dbuf[64];
+    legA.getPosition().print(abuf, 64);
+    legB.getPosition().print(bbuf, 64);
+    legC.getPosition().print(cbuf, 64);
+    legD.getPosition().print(dbuf, 64);
+    snprintf(output, 256, "A = [%s]\nB = [%s]\nC = [%s]\nD = [%s]", abuf, bbuf, cbuf, dbuf);
+    terminal->write(output);
+    return NULL;
+}
+
 
 
 CmdHandler* log(Terminal* terminal, const char* input)
@@ -41,7 +57,7 @@
     if (sscanf(input, "log %d %d", &start, &end) == 1)
     {
         // Print only one item
-        sprintf(output, "%4d: %f\n", start, dataLog[start]);
+        snprintf(output, 256, "%4d: %f\n", start, dataLog[start]);
         terminal->write(output);
     }
     else
@@ -49,7 +65,7 @@
         // Print a range of items
         for (int i = start; i <= end; i++)
         {
-            sprintf(output, "%4d: %f\n", i, dataLog[i]);
+            snprintf(output, 256, "%4d: %f\n", i, dataLog[i]);
             terminal->write(output);
         }
     }  
@@ -96,21 +112,22 @@
     Terminal terminal;
     
     terminal.addCommand("log", &log);
+    terminal.addCommand("leg", &legpos);
     
     radio.reset();
     
     setupLegs();
     
     // Create matrices to change base from robot coordinates to leg coordinates
-    QA.translate(vector3(0.1f, 0.1f, 0.0f));
+    QA.translate(vector3(0.508f, 0.508f, 0.0f));
     PA = QA.inverse();
-    QB.translate(vector3(-0.1f, -0.1f, 0.0f));
+    QB.translate(vector3(-0.508f, -0.508f, 0.0f));
     QB.a11 = -1.0f; QB.a22 = -1.0f;
     PB = QB.inverse();
-    QC.translate(vector3(0.1f, -0.1f, 0.0f));
+    QC.translate(vector3(-0.508f, 0.508f, 0.0f));
     QC.a11 = -1.0f;
     PC = QC.inverse();
-    QD.translate(vector3(-0.1f, 0.1f, 0.0f));
+    QD.translate(vector3(0.508f, -0.508f, 0.0f));
     QD.a22 = -1.0f;
     PD = QD.inverse();
     
@@ -126,6 +143,8 @@
         yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
         turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
         
+        turnaxis = 0.1f;
+        
         // Get delta-time
         deltaTime = deltaTimer.read();
         deltaTimer.reset();
@@ -134,7 +153,7 @@
         // Compute delta movement vector and delta angle
         v.x = -xaxis;
         v.y = -yaxis;
-        v.z = 0;
+        v.z = 0.0f;
         v = v * MAXSPEED * deltaTime;
         angle = -turnaxis * MAXTURN * deltaTime;
         
@@ -214,7 +233,7 @@
     legB.theta.calibrate(980, 1930, 45.0f, -45.0f);
     legB.phi.calibrate(1120, 2030, 70.0f, -45.0f);
     legB.psi.calibrate(2070, 1170, 70.0f, -60.0f);
-    legC.theta.calibrate(2000, 1000, 45.0f, -45.0f);
+    legC.theta.calibrate(1920, 860, 45.0f, -45.0f);
     legC.phi.calibrate(1930, 1050, 70.0f, -45.0f);
     legC.psi.calibrate(1100, 2000, 70.0f, -60.0f);
     legD.theta.calibrate(2000, 1070, 45.0f, -45.0f);