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@17:b77c31807825, 2015-03-17 (annotated)
- Committer:
- mzling
- Date:
- Tue Mar 17 21:16:43 2015 +0000
- Revision:
- 17:b77c31807825
- Parent:
- 16:e69f063a2068
- Child:
- 18:03d7c5fdc0c7
print doesn't work
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 | 13:c588cb388240 | 18 | |
mzling | 13:c588cb388240 | 19 | dataComm *dc = new dataComm(); |
mzling | 13:c588cb388240 | 20 | short int dataIn[32]; |
mzling | 17:b77c31807825 | 21 | short int dataOut[]= {0xFF,30,31,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFE}; |
mzling | 17:b77c31807825 | 22 | bool flag=0; |
cashdollar | 3:9719ad064a2c | 23 | |
mzling | 17:b77c31807825 | 24 | /*union short_or_char { |
mzling | 15:3596b7ca1c20 | 25 | char c[2]; |
mzling | 15:3596b7ca1c20 | 26 | short s; |
mzling | 17:b77c31807825 | 27 | } short_or_char;*/ |
mzling | 15:3596b7ca1c20 | 28 | |
mzling | 15:3596b7ca1c20 | 29 | //short_or_char soc; |
mzling | 15:3596b7ca1c20 | 30 | |
mzling | 15:3596b7ca1c20 | 31 | |
perr1940 | 0:d38d627c922f | 32 | /////////////////////////////////////////////////////////////////////////// |
cashdollar | 3:9719ad064a2c | 33 | |
cashdollar | 3:9719ad064a2c | 34 | // periodicFcns runs at the start of every control loop cycle |
cashdollar | 3:9719ad064a2c | 35 | // It initiates communication with dataBed, checks for errors/safety, and starts the FSM |
perr1940 | 0:d38d627c922f | 36 | void periodicFcns() |
perr1940 | 0:d38d627c922f | 37 | { |
mzling | 17:b77c31807825 | 38 | //pc.printf("Blah\r\n"); |
perr1940 | 2:89b78367d173 | 39 | dataOut[1]=encoder_L.readRaw(); |
cashdollar | 5:498c9bfc56f0 | 40 | dataOut[2]=fsm.error(); |
mzling | 13:c588cb388240 | 41 | int* ptr=(int*)dataIn; |
mzling | 14:8929f5a9837c | 42 | ptr=sendData((int*)dataOut, 21, (int*)dataIn); |
mzling | 17:b77c31807825 | 43 | flag=1; |
mzling | 17:b77c31807825 | 44 | /*for (int i=0;i<21;i++) { |
mzling | 17:b77c31807825 | 45 | pc.printf("%x, ", dataIn[i]); |
mzling | 17:b77c31807825 | 46 | }*/ |
mzling | 17:b77c31807825 | 47 | //pc.printf("\r\n"); |
mzling | 17:b77c31807825 | 48 | //dc->process_write((char*)dataIn,42); |
mzling | 15:3596b7ca1c20 | 49 | |
mzling | 17:b77c31807825 | 50 | //dc->process_write(dataIn, 21); |
cashdollar | 3:9719ad064a2c | 51 | |
perr1940 | 7:d44555be84b7 | 52 | //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]); |
mzling | 17:b77c31807825 | 53 | |
cashdollar | 3:9719ad064a2c | 54 | // Run state change/analysis in FSM |
mzling | 17:b77c31807825 | 55 | int exoState=fsm.state(dataIn[1]); |
mzling | 17:b77c31807825 | 56 | |
perr1940 | 0:d38d627c922f | 57 | } |
perr1940 | 0:d38d627c922f | 58 | |
perr1940 | 0:d38d627c922f | 59 | int main() |
perr1940 | 0:d38d627c922f | 60 | { |
cashdollar | 5:498c9bfc56f0 | 61 | pc.printf("\r\nExoStart \r\n"); |
perr1940 | 0:d38d627c922f | 62 | initializeExoIOs(); |
cashdollar | 5:498c9bfc56f0 | 63 | pc.printf("Test\r\n"); // keep for debugging compile errors |
perr1940 | 2:89b78367d173 | 64 | |
perr1940 | 0:d38d627c922f | 65 | pc.printf("Starting exo...\n\r"); |
cashdollar | 5:498c9bfc56f0 | 66 | |
perr1940 | 0:d38d627c922f | 67 | //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 | 68 | wait(2); |
cashdollar | 5:498c9bfc56f0 | 69 | |
perr1940 | 0:d38d627c922f | 70 | Ticker doControl; |
mzling | 14:8929f5a9837c | 71 | dataBedSPI.format(16,0); |
perr1940 | 0:d38d627c922f | 72 | doControl.attach(&periodicFcns, SAMPLE_TIME); |
perr1940 | 0:d38d627c922f | 73 | |
mzling | 17:b77c31807825 | 74 | while (1) { |
mzling | 17:b77c31807825 | 75 | //pc.printf("!"); |
mzling | 17:b77c31807825 | 76 | if(flag==1) { |
mzling | 17:b77c31807825 | 77 | flag=0; |
mzling | 17:b77c31807825 | 78 | for (int i=0; i<21; i++) { |
mzling | 17:b77c31807825 | 79 | pc.printf("%x, ", dataIn[i]); |
mzling | 17:b77c31807825 | 80 | } |
mzling | 17:b77c31807825 | 81 | } |
mzling | 17:b77c31807825 | 82 | }; |
perr1940 | 0:d38d627c922f | 83 | } |