File content as of revision 54:9d0e871dd00a:

/**************************************
This is the main file for the "RyanEXO" control software.
This sets up the main control loop that operates at a 1kHz frequency
and controls the flow of the rest of the software

The configuration for the exo system that this software runs:
amplifiers: 2 Copley Controls 10A digital amplifiers
sensors: 2 Austria Microsystems 12 bit magnetic encoders, handled by encoders.h
motors: 2 maxon EC90 DC brushless motors, handled by HipMotorControl.h (called in FSM.h)
Knees: wrap spring clutch controlled by Firgelli PQ12P linear actuators, controlled in linearActuatorControl.h
**************************************/

#include "mbed.h"
#include "initExoVars.h"
#include "FSM.h"
#include "dataBedComm.h"
#include "dataComm.h"
short dataIn[13];
/** DataOut: Indices 0,1,2,and 8 are reserved. 0 is start byte, 8 is end byte, 1 and 2 are error codes.
Other indices can be used for read angles */
short dataOut[]= {0xFF,30,31,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFE};
dataComm dc = dataComm();
Timer dbg;
//
///////////////////////////////////////////////////////////////////////////

// periodicFcns runs at the start of every control loop cycle
// It initiates communication with dataBed, checks for errors/safety, and starts the FSM



void periodicFcns()
{
    dbg.reset();
    //pc.printf("%d, ", AvailableMemory());
    //dataOut[1]=encoder_L.readRaw();
    //Get values of left hip angle and torso angle
    float f1 = encoder_L.rereadAngle();
    float f2 = fsm.read_angle_y();

    /** Multiplying by 91 allows casting float to short without losing too much precision
    * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow
    */
    short s1 = (short)(f1*91);
    short s2 = (short)(f2*91);
    dataOut[3] = s1;
    dataOut[4] = s2;
    //pc.printf("1:%f %d\r\n", f1, dataOut[3]);
    //pc.printf("2:%f %d\r\n", f2, dataOut[4]);
    dataOut[2]=fsm.error();
    short* ptr=dataIn;
    ptr=sendData(dataOut, 15, dataIn);
    /*if (dataIn[1] != 0) {
        pc.printf("UI: %d\r\n", dataIn[1]);
    }*/
    //Sends message received from the ctrlbed to the dataComm object
    dc.process_write(dataIn+2, 13);
    //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);

    // Run state change/analysis in FSM
    int exoState=fsm.state(dataIn[1]);//
    //float temp=dbg.read_us();
    //pc.printf("%f\r\n", temp);
    //
}
//

//Starts the Exo controlbed, and processes messages by repeatedly executing periodicFcns
int main()
{//
    pc.baud(921600);//
    pc.printf("\r\nExoStart \r\n");
    wait(1);//

    initializeExoIOs();
    //pc.printf("Test\r\n"); // keep for debugging compile errors
    mbedLED1 = 1;
    pc.printf("Starting exo...\n\r");
    //If desired, a startup sound can be played. This function is defined in the DatabedCode, because it will command a sound to be played once it detects a heartbeat from ControlBed
    wait(2);
    dbg.start();
    Ticker doControl;
    dataBedSPI.format(16,0);
    doControl.attach(&periodicFcns, SAMPLE_TIME);

    while (1);
}