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@29:fb369b8e2493, 2015-05-14 (annotated)

Committer:
perr1940
Date:
Thu May 14 22:52:52 2015 +0000
Revision:
29:fb369b8e2493
Parent:
27:feb56a2ec5c9
Child:
30:1ccef6a5ba50
This works!  The problem was a double delete of a dynamically allocated pointer.  So I'm going to revert back to the class structure and change how we delete to point to the NULL pointer after the fact.

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 26:c4372c518ac3 18 #include "AvailableMemory.h"
mzling 18:03d7c5fdc0c7 19 short dataIn[7];
mzling 24:2e56d3bebb24 20 /** 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 21 Other indices can be used for read angles */
mzling 21:715c8cf78e14 22 short dataOut[]={0xFF,30,31,0, 0, 0, 0, 0, 0xFE};
mzling 18:03d7c5fdc0c7 23 dataComm dc = dataComm();
mzling 18:03d7c5fdc0c7 24 Timer dbg;
mzling 15:3596b7ca1c20 25
perr1940 0:d38d627c922f 26 ///////////////////////////////////////////////////////////////////////////
cashdollar 3:9719ad064a2c 27
cashdollar 3:9719ad064a2c 28 // periodicFcns runs at the start of every control loop cycle
cashdollar 3:9719ad064a2c 29 // It initiates communication with dataBed, checks for errors/safety, and starts the FSM
mzling 23:e0923403be2f 30
mzling 24:2e56d3bebb24 31
mzling 24:2e56d3bebb24 32
perr1940 0:d38d627c922f 33 void periodicFcns()
perr1940 0:d38d627c922f 34 {
mzling 18:03d7c5fdc0c7 35 dbg.reset();
perr1940 27:feb56a2ec5c9 36 //pc.printf("%d, ", AvailableMemory());
mzling 24:2e56d3bebb24 37 //dataOut[1]=encoder_L.readRaw();
mzling 25:1292b886b8d2 38 //Get values of left hip angle and torso angle
mzling 20:cf8e99d989a6 39 float f1 = encoder_L.read_angle();
mzling 20:cf8e99d989a6 40 float f2 = fsm.read_angle_y();
mzling 24:2e56d3bebb24 41
mzling 24:2e56d3bebb24 42 /** Multiplying by 91 allows casting float to short without losing too much precision
mzling 24:2e56d3bebb24 43 * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow
mzling 24:2e56d3bebb24 44 */
mzling 23:e0923403be2f 45 short s1 = (short)(f1*91);
mzling 23:e0923403be2f 46 short s2 = (short)(f2*91);
mzling 24:2e56d3bebb24 47 dataOut[3] = s1;
mzling 24:2e56d3bebb24 48 dataOut[4] = s2;
mzling 24:2e56d3bebb24 49 //pc.printf("1:%f %d\r\n", f1, dataOut[3]);
mzling 24:2e56d3bebb24 50 //pc.printf("2:%f %d\r\n", f2, dataOut[4]);
perr1940 26:c4372c518ac3 51 dataOut[2]=fsm.error();
mzling 18:03d7c5fdc0c7 52 short* ptr=dataIn;
mzling 21:715c8cf78e14 53 ptr=sendData(dataOut, 9, dataIn);
perr1940 26:c4372c518ac3 54 /*if (dataIn[1] != 0) {
mzling 24:2e56d3bebb24 55 pc.printf("UI: %d\r\n", dataIn[1]);
perr1940 26:c4372c518ac3 56 }*/
mzling 25:1292b886b8d2 57 //Sends message received from the ctrlbed to the dataComm object
mzling 21:715c8cf78e14 58 dc.process_write(dataIn+2, 7);
perr1940 7:d44555be84b7 59 //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
mzling 18:03d7c5fdc0c7 60
cashdollar 3:9719ad064a2c 61 // Run state change/analysis in FSM
mzling 17:b77c31807825 62 int exoState=fsm.state(dataIn[1]);
mzling 18:03d7c5fdc0c7 63 float temp=dbg.read_us();
perr1940 29:fb369b8e2493 64 //pc.printf("%f\r\n", temp);
mzling 18:03d7c5fdc0c7 65
perr1940 0:d38d627c922f 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()
perr1940 0:d38d627c922f 70 {
cashdollar 5:498c9bfc56f0 71 pc.printf("\r\nExoStart \r\n");
perr1940 26:c4372c518ac3 72 wait(1);
perr1940 0:d38d627c922f 73 initializeExoIOs();
perr1940 29:fb369b8e2493 74 //pc.printf("Test\r\n"); // keep for debugging compile errors
mzling 20:cf8e99d989a6 75 mbedLED1 = 1;
perr1940 0:d38d627c922f 76 pc.printf("Starting exo...\n\r");
perr1940 0:d38d627c922f 77 //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 78 wait(2);
mzling 18:03d7c5fdc0c7 79 dbg.start();
perr1940 0:d38d627c922f 80 Ticker doControl;
mzling 14:8929f5a9837c 81 dataBedSPI.format(16,0);
perr1940 0:d38d627c922f 82 doControl.attach(&periodicFcns, SAMPLE_TIME);
perr1940 0:d38d627c922f 83
mzling 18:03d7c5fdc0c7 84 while (1);
perr1940 0:d38d627c922f 85 }