File content as of revision 1:a5447cf54fba:

#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* step(Terminal* terminal, const char* input)
{
    float x, y, z;
    char output[256];
    
    if (sscanf(input, "step %f %f %f", &x, &y, &z) == 3)
    {
        leg.step(x, y, z);
        sprintf(output, "stepping to new position");
    }
    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("step", &step);
    
    while(true)
    {
        leg.update();
    }
}