File content as of revision 3:6fa07ceb897f:

#include "mbed.h"
#include "RobotLeg.h"
#include "Matrix.h"
#include "Terminal.h"
#include "CircularBuffer.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;
}



int main()
{
    Timer timer;
    float angle, arc; // radius, speed are globals now
    vector3 v;
    matrix4 P, Q, T; 
    Terminal terminal;
    
    terminal.addCommand("log", &log);
    terminal.addCommand("speed", &setspeed);
    terminal.addCommand("radius", &setradius);
    
    // 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);
    
    // Create matrices to change base from robot coordinates to leg coordinates
    Q.translate(vector3(0.08f, 0.08f, 0.0f));
    P = Q.inverse();

    // Go to initial position
    leg.step(vector3(0.1f, 0.1f, -0.05f));
    
    // Set radius and speed to go straight forward
    radius = 0.0f; // meters
    speed = 0.0f; // m/s
    
    timer.start();
    
    /*
    // Dump debug info 
    sprintf(output, "T =\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t0\t\t0\t\t0\t\t1\n",
            T.a11, T.a12, T.a13, T.a14,
            T.a21, T.a22, T.a23, T.a24,
            T.a31, T.a32, T.a33, T.a34);
    terminal.write(output); */
    
    
    while(true)
    {
        arc = speed*timer.read();
        dataLog.push(timer.read());
        timer.reset();
        
        if (radius == 0)
        {
            angle = 0.0f;
            v.x = 0.0f;
            v.y = arc;
            v.z = 0.0f;
        }
        else
        {
            angle = -arc/radius;
            v.x = radius*(1 - cos(-angle));
            v.y = radius*sin(-angle);
            v.z = 0.0f;
        }
        
        T.identity().rotateZ(angle).translate(v);
    
        leg.update(P*T.inverse()*Q);
    }
}