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@21:715c8cf78e14, 2015-04-15 (annotated)

Committer:
mzling
Date:
Wed Apr 15 01:51:46 2015 +0000
Revision:
21:715c8cf78e14
Parent:
20:cf8e99d989a6
Child:
22:357327fe25af
Added UI receiving capability

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"
mzling 18:03d7c5fdc0c7 18 short dataIn[7];
mzling 21:715c8cf78e14 19 short dataOut[]={0xFF,30,31,0, 0, 0, 0, 0, 0xFE};
mzling 18:03d7c5fdc0c7 20 dataComm dc = dataComm();
mzling 18:03d7c5fdc0c7 21 Timer dbg;
mzling 15:3596b7ca1c20 22
perr1940 0:d38d627c922f 23 ///////////////////////////////////////////////////////////////////////////
cashdollar 3:9719ad064a2c 24
cashdollar 3:9719ad064a2c 25 // periodicFcns runs at the start of every control loop cycle
cashdollar 3:9719ad064a2c 26 // It initiates communication with dataBed, checks for errors/safety, and starts the FSM
perr1940 0:d38d627c922f 27 void periodicFcns()
perr1940 0:d38d627c922f 28 {
mzling 18:03d7c5fdc0c7 29 dbg.reset();
mzling 20:cf8e99d989a6 30 // dataOut[1]=encoder_L.readRaw();
mzling 20:cf8e99d989a6 31 float f1 = encoder_L.read_angle();
mzling 20:cf8e99d989a6 32 float f2 = fsm.read_angle_y();
mzling 20:cf8e99d989a6 33 dataOut[1] = (short)f1;
mzling 20:cf8e99d989a6 34 dataOut[2] = (short)f1;
mzling 20:cf8e99d989a6 35 dataOut[3] = (short)f1;
mzling 20:cf8e99d989a6 36 dataOut[4] = (short)f2;
mzling 20:cf8e99d989a6 37 dataOut[5] = (short)f2;
mzling 20:cf8e99d989a6 38 //pc.printf("%f\r\n", f1);
mzling 21:715c8cf78e14 39 //pc.printf("%d\r\n", (short)f1);
mzling 20:cf8e99d989a6 40 //pc.printf("%f\r\n", f2);
mzling 21:715c8cf78e14 41 //pc.printf("%d\r\n", (short)f2);
mzling 20:cf8e99d989a6 42 //for (int i = 2; i < 4; i++) {
mzling 20:cf8e99d989a6 43 // pc.printf("data%d is %x\r\n", i, dataOut[i]);
mzling 20:cf8e99d989a6 44 //}
mzling 18:03d7c5fdc0c7 45 //dataOut[2]=fsm.error();
mzling 18:03d7c5fdc0c7 46 short* ptr=dataIn;
mzling 21:715c8cf78e14 47 ptr=sendData(dataOut, 9, dataIn);
mzling 21:715c8cf78e14 48 //for (int i = 0; i < 7; i++) {
mzling 21:715c8cf78e14 49 if (dataIn[1] != 0) {
mzling 21:715c8cf78e14 50 pc.printf("UI: %d\r\n", dataIn[1]);
mzling 18:03d7c5fdc0c7 51 }
mzling 21:715c8cf78e14 52 //}
mzling 21:715c8cf78e14 53 //pc.printf("\r\n");*/
mzling 21:715c8cf78e14 54 dc.process_write(dataIn+2, 7);
perr1940 7:d44555be84b7 55 //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
mzling 18:03d7c5fdc0c7 56
cashdollar 3:9719ad064a2c 57 // Run state change/analysis in FSM
mzling 17:b77c31807825 58 int exoState=fsm.state(dataIn[1]);
mzling 18:03d7c5fdc0c7 59 float temp=dbg.read_us();
mzling 21:715c8cf78e14 60 //pc.printf("%f\r\n",temp);
mzling 18:03d7c5fdc0c7 61
perr1940 0:d38d627c922f 62 }
perr1940 0:d38d627c922f 63
mzling 20:cf8e99d989a6 64 //Starts the Exo controlbed, and processes messages by repeatedly executing periodicFcns
perr1940 0:d38d627c922f 65 int main()
perr1940 0:d38d627c922f 66 {
cashdollar 5:498c9bfc56f0 67 pc.printf("\r\nExoStart \r\n");
perr1940 0:d38d627c922f 68 initializeExoIOs();
mzling 18:03d7c5fdc0c7 69 //pc.printf("Test\r\n"); // keep for debugging compile errors
mzling 20:cf8e99d989a6 70 mbedLED1 = 1;
perr1940 0:d38d627c922f 71 pc.printf("Starting exo...\n\r");
perr1940 0:d38d627c922f 72 //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 73 wait(2);
mzling 18:03d7c5fdc0c7 74 dbg.start();
perr1940 0:d38d627c922f 75 Ticker doControl;
mzling 14:8929f5a9837c 76 dataBedSPI.format(16,0);
perr1940 0:d38d627c922f 77 doControl.attach(&periodicFcns, SAMPLE_TIME);
perr1940 0:d38d627c922f 78
mzling 18:03d7c5fdc0c7 79 while (1);
perr1940 0:d38d627c922f 80 }