File content as of revision 5:475f67175510:

#include "mbed.h"
#include "RobotLeg.h"
#include "Matrix.h"
#include "CircularBuffer.h"
#include "Radio.h"
#include "commands.h"
#include <cstring>



CircularBuffer<float,16> dataLog;
float radius, speed;
RobotLeg legA(p26, p29, p30);
RobotLeg legB(p13, p14, p15);
RobotLeg legC(p12, p11, p8);
RobotLeg legD(p23, p24, p25);
Radio radio(p5, p6, p7, p17, p19, p20);



int main()
{
    Timer deltaTimer;
    Timer cycleTimer;
    float angle, arc;
    vector3 v;
    matrix4 T;
    matrix4 PA, QA;
    matrix4 PB, QB; 
    matrix4 PC, QC; 
    matrix4 PD, QD; 
    Terminal terminal;
    
    terminal.addCommand("log", &log);
    terminal.addCommand("speed", &setspeed);
    terminal.addCommand("radius", &setradius);
    terminal.addCommand("read", &read);
    
    // Set leg parameters
    legA.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
    legB.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
    legC.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
    legD.setDimensions(0.11f, 0.125f, -0.006f, 0.020f);
    legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
    legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
    legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
    legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
    legA.setStepCircle(0.12, 0.11, -0.15, 0.11);
    legB.setStepCircle(0.12, 0.11, -0.15, 0.11);
    legC.setStepCircle(0.12, 0.11, -0.15, 0.11);
    legD.setStepCircle(0.12, 0.11, -0.15, 0.11);
    legA.theta.calibrate(1000, 2000, 45.0f, -45.0f);
    legA.phi.calibrate(1000, 2000, 70.0f, -45.0f);
    legA.psi.calibrate(2000, 1000, 80.0f, -45.0f);
    legB.theta.calibrate(1000, 2000, 45.0f, -45.0f);
    legB.phi.calibrate(1000, 2000, 70.0f, -45.0f);
    legB.psi.calibrate(2000, 1000, 80.0f, -45.0f);
    legC.theta.calibrate(2000, 1000, 45.0f, -45.0f);
    legC.phi.calibrate(2000, 1000, 70.0f, -45.0f);
    legC.psi.calibrate(1000, 2000, 80.0f, -45.0f);
    legD.theta.calibrate(2000, 1000, 45.0f, -45.0f);
    legD.phi.calibrate(2000, 1000, 70.0f, -45.0f);
    legD.psi.calibrate(1000, 2000, 80.0f, -45.0f);

    // Create matrices to change base from robot coordinates to leg coordinates
    QA.translate(vector3(0.1f, 0.1f, 0.0f));
    PA = QA.inverse();
    QB.translate(vector3(-0.1f, -0.1f, 0.0f));
    QB.a11 = -1.0f; QB.a22 = -1.0f;
    PB = QB.inverse();
    QC.translate(vector3(0.1f, -0.1f, 0.0f));
    QC.a11 = -1.0f;
    PC = QC.inverse();
    QD.translate(vector3(-0.1f, 0.1f, 0.0f));
    QD.a22 = -1.0f;
    PD = QD.inverse();

    // Go to initial position
    legA.reset(-0.5f);
    legB.reset(0.0f);
    legC.reset(0.5f);
    legD.reset(1.0f);
    
    // Set radius and speed to go straight forward
    radius = 0.0f; // meters
    speed = 0.0f; // m/s
    
    deltaTimer.start();
    cycleTimer.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)
    {
        // Get arc length of movement delta
        arc = speed*deltaTimer.read();
        dataLog.push(deltaTimer.read());
        deltaTimer.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;
        }
        
        // Compute movement transformation in robot coordinates
        T.identity().rotateZ(angle).translate(v).inverse();
    
        legA.update(PA*T*QA);
        legB.update(PB*T*QB);
        legC.update(PC*T*QC);
        legD.update(PD*T*QD);
    }
}