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@41:68f130768877, 2015-06-26 (annotated)
- Committer:
- perr1940
- Date:
- Fri Jun 26 21:40:20 2015 +0000
- Revision:
- 41:68f130768877
- Parent:
- 40:77fa783b97f3
- Child:
- 44:5f0dfbbce4f4
updated to latest version. No dynamic memory allocation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
perr1940 | 0:d38d627c922f | 1 | /************************************** |
perr1940 | 0:d38d627c922f | 2 | This is the main file for the "RyanEXO" control software. |
perr1940 | 0:d38d627c922f | 3 | This sets up the main control loop that operates at a 1kHz frequency |
perr1940 | 0:d38d627c922f | 4 | and controls the flow of the rest of the software |
perr1940 | 0:d38d627c922f | 5 | |
perr1940 | 0:d38d627c922f | 6 | The configuration for the exo system that this software runs: |
perr1940 | 0:d38d627c922f | 7 | amplifiers: 2 Copley Controls 10A digital amplifiers |
perr1940 | 0:d38d627c922f | 8 | sensors: 2 Austria Microsystems 12 bit magnetic encoders, handled by encoders.h |
perr1940 | 0:d38d627c922f | 9 | motors: 2 maxon EC90 DC brushless motors, handled by HipMotorControl.h (called in FSM.h) |
perr1940 | 0:d38d627c922f | 10 | Knees: wrap spring clutch controlled by Firgelli PQ12P linear actuators, controlled in linearActuatorControl.h |
cashdollar | 3:9719ad064a2c | 11 | **************************************/ |
perr1940 | 0:d38d627c922f | 12 | |
perr1940 | 0:d38d627c922f | 13 | #include "mbed.h" |
perr1940 | 0:d38d627c922f | 14 | #include "initExoVars.h" |
perr1940 | 0:d38d627c922f | 15 | #include "FSM.h" |
perr1940 | 2:89b78367d173 | 16 | #include "dataBedComm.h" |
mzling | 13:c588cb388240 | 17 | #include "dataComm.h" |
mzling | 18:03d7c5fdc0c7 | 18 | short dataIn[7]; |
mzling | 24:2e56d3bebb24 | 19 | /** DataOut: Indices 0,1,2,and 8 are reserved. 0 is start byte, 8 is end byte, 1 and 2 are error codes. |
mzling | 24:2e56d3bebb24 | 20 | Other indices can be used for read angles */ |
cashdollar | 38:d472f42506f7 | 21 | short dataOut[]= {0xFF,30,31,0, 0, 0, 0, 0, 0xFE}; |
mzling | 18:03d7c5fdc0c7 | 22 | dataComm dc = dataComm(); |
mzling | 18:03d7c5fdc0c7 | 23 | Timer dbg; |
cashdollar | 37:37ba5cb94bec | 24 | // |
perr1940 | 0:d38d627c922f | 25 | /////////////////////////////////////////////////////////////////////////// |
cashdollar | 3:9719ad064a2c | 26 | |
cashdollar | 3:9719ad064a2c | 27 | // periodicFcns runs at the start of every control loop cycle |
cashdollar | 3:9719ad064a2c | 28 | // It initiates communication with dataBed, checks for errors/safety, and starts the FSM |
mzling | 23:e0923403be2f | 29 | |
mzling | 24:2e56d3bebb24 | 30 | |
mzling | 24:2e56d3bebb24 | 31 | |
perr1940 | 0:d38d627c922f | 32 | void periodicFcns() |
perr1940 | 0:d38d627c922f | 33 | { |
mzling | 18:03d7c5fdc0c7 | 34 | dbg.reset(); |
perr1940 | 27:feb56a2ec5c9 | 35 | //pc.printf("%d, ", AvailableMemory()); |
mzling | 24:2e56d3bebb24 | 36 | //dataOut[1]=encoder_L.readRaw(); |
mzling | 25:1292b886b8d2 | 37 | //Get values of left hip angle and torso angle |
perr1940 | 36:6e671fa75dd2 | 38 | float f1 = encoder_L.rereadAngle(); |
mzling | 20:cf8e99d989a6 | 39 | float f2 = fsm.read_angle_y(); |
cashdollar | 38:d472f42506f7 | 40 | |
mzling | 24:2e56d3bebb24 | 41 | /** Multiplying by 91 allows casting float to short without losing too much precision |
mzling | 24:2e56d3bebb24 | 42 | * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow |
mzling | 24:2e56d3bebb24 | 43 | */ |
mzling | 23:e0923403be2f | 44 | short s1 = (short)(f1*91); |
mzling | 23:e0923403be2f | 45 | short s2 = (short)(f2*91); |
mzling | 24:2e56d3bebb24 | 46 | dataOut[3] = s1; |
mzling | 24:2e56d3bebb24 | 47 | dataOut[4] = s2; |
mzling | 24:2e56d3bebb24 | 48 | //pc.printf("1:%f %d\r\n", f1, dataOut[3]); |
mzling | 24:2e56d3bebb24 | 49 | //pc.printf("2:%f %d\r\n", f2, dataOut[4]); |
perr1940 | 26:c4372c518ac3 | 50 | dataOut[2]=fsm.error(); |
mzling | 18:03d7c5fdc0c7 | 51 | short* ptr=dataIn; |
mzling | 21:715c8cf78e14 | 52 | ptr=sendData(dataOut, 9, dataIn); |
cashdollar | 38:d472f42506f7 | 53 | /*if (dataIn[1] != 0) { |
mzling | 24:2e56d3bebb24 | 54 | pc.printf("UI: %d\r\n", dataIn[1]); |
perr1940 | 26:c4372c518ac3 | 55 | }*/ |
mzling | 25:1292b886b8d2 | 56 | //Sends message received from the ctrlbed to the dataComm object |
mzling | 21:715c8cf78e14 | 57 | dc.process_write(dataIn+2, 7); |
perr1940 | 7:d44555be84b7 | 58 | //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]); |
cashdollar | 38:d472f42506f7 | 59 | |
cashdollar | 3:9719ad064a2c | 60 | // Run state change/analysis in FSM |
cashdollar | 37:37ba5cb94bec | 61 | int exoState=fsm.state(dataIn[1]);// |
perr1940 | 30:1ccef6a5ba50 | 62 | //float temp=dbg.read_us(); |
perr1940 | 29:fb369b8e2493 | 63 | //pc.printf("%f\r\n", temp); |
cashdollar | 37:37ba5cb94bec | 64 | // |
perr1940 | 0:d38d627c922f | 65 | } |
cashdollar | 37:37ba5cb94bec | 66 | // |
perr1940 | 0:d38d627c922f | 67 | |
mzling | 20:cf8e99d989a6 | 68 | //Starts the Exo controlbed, and processes messages by repeatedly executing periodicFcns |
perr1940 | 0:d38d627c922f | 69 | int main() |
cashdollar | 38:d472f42506f7 | 70 | {// |
cashdollar | 38:d472f42506f7 | 71 | pc.baud(921600);// |
cashdollar | 5:498c9bfc56f0 | 72 | pc.printf("\r\nExoStart \r\n"); |
cashdollar | 38:d472f42506f7 | 73 | wait(1);// |
cashdollar | 37:37ba5cb94bec | 74 | |
perr1940 | 0:d38d627c922f | 75 | initializeExoIOs(); |
perr1940 | 41:68f130768877 | 76 | pc.printf("Test\r\n"); // keep for debugging compile errors |
mzling | 20:cf8e99d989a6 | 77 | mbedLED1 = 1; |
perr1940 | 0:d38d627c922f | 78 | pc.printf("Starting exo...\n\r"); |
perr1940 | 0:d38d627c922f | 79 | //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 |
perr1940 | 0:d38d627c922f | 80 | wait(2); |
mzling | 18:03d7c5fdc0c7 | 81 | dbg.start(); |
perr1940 | 0:d38d627c922f | 82 | Ticker doControl; |
mzling | 14:8929f5a9837c | 83 | dataBedSPI.format(16,0); |
perr1940 | 0:d38d627c922f | 84 | doControl.attach(&periodicFcns, SAMPLE_TIME); |
perr1940 | 0:d38d627c922f | 85 | |
mzling | 18:03d7c5fdc0c7 | 86 | while (1); |
perr1940 | 0:d38d627c922f | 87 | } |