File content as of revision 0:449568595ed9:

#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) {}
}