main.cpp

00001 /**************************************
00002 This is the main file for the "RyanEXO" control software.
00003 This sets up the main control loop that operates at a 1kHz frequency
00004 and controls the flow of the rest of the software
00005 
00006 The configuration for the exo system that this software runs:
00007 amplifiers: 2 Copley Controls 10A digital amplifiers
00008 sensors: 2 Austria Microsystems 12 bit magnetic encoders, handled by encoders.h
00009 motors: 2 maxon EC90 DC brushless motors, handled by HipMotorControl.h (called in FSM.h)
00010 Knees: wrap spring clutch controlled by Firgelli PQ12P linear actuators, controlled in linearActuatorControl.h
00011 **************************************/
00012 
00013 #include "mbed.h"
00014 #include "initExoVars.h"
00015 #include "FSM.h"
00016 #include "dataBedComm.h"
00017 #include "dataComm.h"
00018 short dataIn[13];
00019 /** DataOut: Indices 0,1,2,and 8 are reserved. 0 is start byte, 8 is end byte, 1 and 2 are error codes.
00020 Other indices can be used for read angles */
00021 short dataOut[]= {0xFF,30,31,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFE};
00022 dataComm dc = dataComm();
00023 Timer dbg;
00024 //
00025 ///////////////////////////////////////////////////////////////////////////
00026 
00027 // periodicFcns runs at the start of every control loop cycle
00028 // It initiates communication with dataBed, checks for errors/safety, and starts the FSM
00029 
00030 
00031 
00032 void periodicFcns()
00033 {
00034     dbg.reset();
00035     //pc.printf("%d, ", AvailableMemory());
00036     //dataOut[1]=encoder_L.readRaw();
00037     //Get values of left hip angle and torso angle
00038     float f1 = encoder_L.rereadAngle();
00039     float f2 = fsm.read_angle_y();
00040 
00041     /** Multiplying by 91 allows casting float to short without losing too much precision
00042     * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow
00043     */
00044     short s1 = (short)(f1*91);
00045     short s2 = (short)(f2*91);
00046     dataOut[3] = s1;
00047     dataOut[4] = s2;
00048     //pc.printf("1:%f %d\r\n", f1, dataOut[3]);
00049     //pc.printf("2:%f %d\r\n", f2, dataOut[4]);
00050     dataOut[2]=fsm.error();
00051     short* ptr=dataIn;
00052     ptr=sendData(dataOut, 15, dataIn);
00053     /*if (dataIn[1] != 0) {
00054         pc.printf("UI: %d\r\n", dataIn[1]);
00055     }*/
00056     //Sends message received from the ctrlbed to the dataComm object
00057     dc.process_write(dataIn+2, 13);
00058     //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
00059 
00060     // Run state change/analysis in FSM
00061     int exoState=fsm.state(dataIn[1]);//
00062     //float temp=dbg.read_us();
00063     //pc.printf("%f\r\n", temp);
00064     //
00065 }
00066 //
00067 
00068 //Starts the Exo controlbed, and processes messages by repeatedly executing periodicFcns
00069 int main()
00070 {//
00071     pc.baud(921600);//
00072     pc.printf("\r\nExoStart \r\n");
00073     wait(1);//
00074 
00075     initializeExoIOs();
00076     //pc.printf("Test\r\n"); // keep for debugging compile errors
00077     mbedLED1 = 1;
00078     pc.printf("Starting exo...\n\r");
00079     //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
00080     wait(2);
00081     dbg.start();
00082     Ticker doControl;
00083     dataBedSPI.format(16,0);
00084     doControl.attach(&periodicFcns, SAMPLE_TIME);
00085 
00086     while (1);
00087 }