Diff: main.cpp

Revision:

0:449568595ed9

Child:

1:a5447cf54fba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 21 03:31:40 2012 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+#include "RobotLeg.h"
+#include "Terminal.h"
+#include <cstring>
+
+
+
+RobotLeg leg(p17, p15, p13);
+
+
+
+CmdHandler* move(Terminal* terminal, const char* input)
+{
+    float x, y, z;
+    bool success = false;
+    char output[256];
+    
+    if (sscanf(input, "move %f %f %f", &x, &y, &z) == 3)
+    {
+        success = leg.move(x, y, z);
+    }
+    
+    if (success)
+        sprintf(output, "new angles: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
+    else
+        sprintf(output, "invalid position: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
+    terminal->write(output);
+    
+    return NULL;
+}
+
+
+
+CmdHandler* test(Terminal* terminal, const char* input)
+{
+    char arg[32];
+    char output[256];
+    
+    if (sscanf(input, "test %s", arg) == 1)
+    {
+        if (strncmp(arg, "theta", 8))
+        {
+            sprintf(output, "%u %7.2f %7.2f %7.2f", leg.theta.center, leg.theta.usPerDegree, leg.theta.upperLimit, leg.theta.lowerLimit);
+        }
+        else if (strncmp(arg, "phi", 8))
+        {
+            sprintf(output, "%u %7.2f %7.2f %7.2f", leg.phi.center, leg.phi.usPerDegree, leg.phi.upperLimit, leg.phi.lowerLimit);
+        }
+        else if (strncmp(arg, "psi", 8))
+        {
+            sprintf(output, "%u %7.2f %7.2f %7.2f", leg.psi.center, leg.psi.usPerDegree, leg.psi.upperLimit, leg.psi.lowerLimit);
+        }
+        else
+        {
+            sprintf(output, "%s is not a valid servo", arg);
+        }
+    }
+    else
+    {
+        sprintf(output, "error reading input parameters");
+    }
+    
+    terminal->write(output);
+    
+    return NULL;
+}
+
+
+
+int main()
+{
+    Terminal terminal;
+
+    // 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);
+    
+    terminal.addCommand("move", &move);
+    terminal.addCommand("test", &test);
+    
+    while(true) {}
+}