File content as of revision 2:ca6d8d1f77d4:

#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;
    LowPowerTimer time;
    //log.init("logfile", true);
    time.start();
    inittime = time.read();
/*
    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0)
    {
        Ordi->log( "ERROR in SDL init: " << SDL_GetError() );
        return -1;
    }
    atexit(SDL_Quit);
    screen = SDL_SetVideoMode(800, 480, 32, SDL_NOFRAME | SDL_FULLSCREEN);
    SDL_ShowCursor(0);
    SDL_JoystickEventState(SDL_ENABLE);
    // sort out joystick control
    if (SDL_NumJoysticks()>0)
    {
        joy=SDL_JoystickOpen(0);
        if (joy)
        {
            Ordi->log( "Joystick Connected." );
        } else {
            Ordi->log( "ERROR: could not connect to joystick!" );
            return -1;
        }
    } else {
        Ordi->log("ERROR: could not find joystick!" );
        return -1;
    }
*/
    // UDOO to Due is /dev/ttymxc3
    ser.init_old("/dev/ttymxc3", false);
    
    Ordi->log("Press Start to connect." );
    cont = false;
    while (!cont)
    {
        while (SDL_PollEvent(&event))
        {
            switch (event.type)
            {
                case SDL_JOYBUTTONDOWN:
                    if (event.jbutton.button == 7) cont = true;
                    break;
            }
        }
        SDL_Delay(10);
    }

    // prep the motor enable / disable packets
    pack_enable = new packet(18, 'A');
    pack_enable->data[0] = 0x03;
    pack_disable = new packet(18, 'A');
    pack_disable->data[0] = 0x03;
    for (ii=0; ii<18; ii++)
    {
        pack_enable->data[ii+1] = 0x01;
        pack_disable->data[ii+1] = 0x00;
    }
    
    // before continuing, ask scontroller for servo data
    // mostly to make sure it's ready to do stuff
    Ordi->log("Confirming Connection.." );
    // step 1: create a packet destined for the arbotix-m
    pack = new packet(16, 'A'); // reasonable size?
    // step 2: set command byte to 0x05, request for data
    pack->data[0] = 0x05;
    // step 3: set request byte to 0x01, requesting servo angles
    pack->data[1] = 0x01;
    // step 4: set return byte to 'D', send back to Due
    pack->data[2] = 'U';
    // step 5: send the packet
    ser.send(pack, true);
    sleep(1);
    
    // the Arbotix-M might be off or initializing, so keep sending the request
    // until you hear something back
    pack2 = NULL;
    while ((pack2 = ser.recv('U', false)) == NULL)
    {
        ser.send(pack, true);
        sleep(1);
    }
    delete pack;
    
    // we received data from the Arbotix-M, it's good to continue
    Ordi->log( "System Ready." );
    for (ii=0; ii<18; ii++)
    {
        memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)), 
                sizeof(float));
        Ordi->log( pos );
        hex.servoangle[ii] = pos;
    }
    hex.setAngles();
    delete pack2;

//rien qui ne nous interesse jusque ici
    Ordi->log( "Angles read." );
    
    // sit / stand
    dsize = 100;
    psize = SIZE;
    pack = new packet(dsize, 'A', psize);
    pack->data[0] = 0x01; // set servo positions

    // 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

    //ser.send(pack_enable);

    // get ready to ask for data
    pack_ask = new packet(16, 'A');
    pack_ask->data[0] = 0x05;
    pack_ask->data[1] = 0x02; // want temperature
    pack_ask->data[2] = 'U';
    pack_data = NULL;
    
    Ordi->log( "Begin IK" );
    // IK test
    time = 0.0;
    lasttime = time.read();
    hex.safeStand();
    while (hex.ssrunning)
    {
        dt = (getTime() - lasttime);
        lasttime = getTime();
        hex.step(dt);
        // package positions
        for (ii=0; ii<18; ii++)
        {
            pos = hex.servoangle[ii];
            memcpy(pack->data+1+ii*sizeof(float), 
                    &pos, sizeof(float));
        }
        ser.send(pack);
        usleep(20*1000);

    }
    lasttime = getTime();
    lastdata = lasttime;
    ser.send(pack_ask);
    usleep(20*1000);

    quit = false;
    while (!quit)
    {
        dt = (getTime() - lasttime);
        time += dt;
        lasttime = getTime();
        hex.step(dt);
        d = new data_chunk('P', inittime);
        d->add(hex.dr_ang);
        d->add(hex.dr_xpos);
        d->add(hex.dr_ypos);
        d->add(hex.maxsweep);
        d->add(hex.speedmodifier);
        log.send(d);
        // package positions
        for (ii=0; ii<18; ii++)
        {
            pos = hex.servoangle[ii];
            memcpy(pack->data+1+(ii)*sizeof(float), 
                    &pos, sizeof(float));
        }
        ser.send(pack);
        // ask for data?
        
        if (getTime() - lastdata > 1.0)
        {
        if ((pack2=ser.recv('U',false)) != NULL)
        {
            maxval = 1e-10;
            avgtemp = 0.0;
            for (ii=0; ii<18; ii++)
            {
                memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)), 
                        sizeof(float));
                memcpy(&errcode, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t))+sizeof(float), sizeof(uint8_t));
                avgtemp += pos;
                if (pos > maxval) maxval = pos;
                if (errcode != 0) // 32=overload, 4=temperature, 1=voltage
                    Ordi->log( "SERVO ERROR: "<< (int)(errcode) << " ON SERVO " << ii );
            }
            avgtemp /= 18.;
            d = new data_chunk('T', inittime);
            d->add(avgtemp);
            log.send(d);
            delete pack2;
            lastdata = getTime();
            pack2 = NULL;
            ser.send(pack_ask);
            usleep(20*1000);
        }
        }
        
        while (SDL_PollEvent(&event))
        {
            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);
    delete pack;
    log.close();
    ser.close();
    return 0;
    }