File content as of revision 3:13bd725bd47b:

#include "mbed.h"
#include "COM/SSC-32.h"
#include "COM/LOGGER.h"
#include "LIB/Hexapod.h"

static Logger *Ordi = Logger::Instance();
int main()
{
    //0.declaration des ressources    

    int ii, ij, ik, ix, iy, size, ind;
    int cx, cy, attempts;
    //serial ser;
    packet *pack, *pack2, *pack_ask, *pack_data;
    packet *pack_enable, *pack_disable;
    hexapod hex;
    data_chunk *d;
    //logger log;
    //SDL_Surface *screen;
    //SDL_Event event;
    //SDL_Joystick *joy;
    int dsize, psize;
    double time, lasttime, dt, lastdata, inittime;
    uint8_t errcode;
    float pos, avgtemp, joyval, maxval;
    unsigned char chk;
    bool cont, quit;
    //My adds
    LowPowerTimer time;
    SSC_32 ser;
    //log.init("logfile", true);
    time.start();
    inittime = time.read();

    // max useable speed is 2.0 -> 1 foot per second
    hex.speed = 0.0; // in cycles per second
    hex.turning = 0.0; // [-1,1], rotation in z-axis

    Ordi->log( "Begin IK" );
    // IK test
    time = 0.0;
    lasttime = time.read();
    hex.safeStand();
    while (hex.ssrunning)
    {
        dt = (time.read() - lasttime);
        lasttime = time.read();
        hex.step(dt);
        // package positions
        for (ii=0; ii<18; ii++)
        {
            ser.add(ii,hex.servoangle[ii]);
            
        }
        ser.send();
        wait_us(20*1000);

    }
    lasttime = time.read();
    lastdata = lasttime;
    ser.send();
    wait_us(20*1000);

    quit = false;
    while (!quit)
    {
        dt = (time.read() - lasttime);
        time += dt;
        lasttime = time.read();
        hex.step(dt);
        // package positions
        for (ii=0; ii<18; ii++)
        {
            ser.add(ii, hex.servoangle[ii]);
        }
        ser.send();
        // ask for data?        
        
        while (Ordi->buffer_has_recived()!=false)
        {
            switch (event.type)
            {
                case SDL_JOYBUTTONDOWN:
                    if (event.jbutton.button == 7) quit = true;
                    break;
                case SDL_JOYAXISMOTION:
                    joyval = -event.jaxis.value/32767.;
                    if (event.jaxis.axis == 1) // L stick, yaxis
                    {
                        if (joyval > 0.1) hex.speed = 0.5*(joyval - 0.1);
                        else if (joyval < -0.1) hex.speed = 0.5*(joyval + 0.1);
                        else hex.speed = 0.0;
                    }
                    if (event.jaxis.axis == 2) // R stick, xaxis
                    {
                        if (joyval > 0.1) hex.turning = (joyval-0.1);
                        else if (joyval < -0.1) hex.turning = (joyval+0.1);
                        else hex.turning = 0.0;
                    }
                    if (event.jaxis.axis == 3) // R stick, yaxis
                    {
                        if (joyval > 0.1) hex.standheight = (joyval-0.1)*2.0;
                        else if (joyval < -0.1) hex.standheight = (joyval+0.1)*2.0;
                        else hex.standheight = 0.0;
                    }
                    break;
            }
        }
        SDL_Delay(20);
    }

    Ordi->log( "Quitting.." );
    ser.send(pack_disable);
    ser.close();
    return 0;
    }