Nathaniel Honka / Mbed 2 deprecated Motion-Control

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 HEL's Angels

main.cpp@54:9d0e871dd00a, 2015-07-02 (annotated)

Committer:
nathanhonka
Date:
Thu Jul 02 21:42:11 2015 +0000
Revision:
54:9d0e871dd00a
Parent:
52:9cd42eadea31 
Corrected header file include guards.

Who changed what in which revision?

UserRevisionLine numberNew 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"
perr1940 52:9cd42eadea31 18 short dataIn[13];
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 */
perr1940 52:9cd42eadea31 21 short dataOut[]= {0xFF,30,31,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFE};
mzling 18:03d7c5fdc0c7 22 dataComm dc = dataComm();
mzling 18:03d7c5fdc0c7 23 Timer dbg;
cashdollar 51:b0a6dc4aece1 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;
perr1940 52:9cd42eadea31 52 ptr=sendData(dataOut, 15, 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
perr1940 52:9cd42eadea31 57 dc.process_write(dataIn+2, 13);
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 44:5f0dfbbce4f4 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 }