Corrected header file include guards.
Dependencies: FiniteStateMachine HipControl Knee LinearBlend1 LocalFileSystem_Read dataComm hapticFeedback initExoVars mbed Blend_Generator Brad_poly_gait Gait_Generator MM_gait Encoders IMUdriver
Fork of Motion Control by
main.cpp
- Committer:
- perr1940
- Date:
- 2015-06-29
- Revision:
- 44:5f0dfbbce4f4
- Parent:
- 41:68f130768877
- Child:
- 49:2d4ec945cd9c
File content as of revision 44:5f0dfbbce4f4:
/************************************** 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[7]; /** 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, 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, 9, 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, 7); //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); }