Diff: main.cpp

Revision:

26:c4372c518ac3

Parent:

25:1292b886b8d2

Child:

27:feb56a2ec5c9

--- a/main.cpp	Mon May 11 21:47:16 2015 +0000
+++ b/main.cpp	Wed May 13 19:24:10 2015 +0000
@@ -15,6 +15,7 @@
 #include "FSM.h"
 #include "dataBedComm.h"
 #include "dataComm.h"
+#include "AvailableMemory.h"
 short dataIn[7];
 /** DataOut: Indices 0,1,2,and 8 are reserved. 0 is start byte, 8 is end byte, 1 and 2 are error codes.
 Other indices can be used for read angles */
@@ -32,6 +33,7 @@
 void periodicFcns()
 {
     dbg.reset();
+    //pc.printf("%d, ", AvailableMemory(1));
     //dataOut[1]=encoder_L.readRaw();
     //Get values of left hip angle and torso angle
     float f1 = encoder_L.read_angle();
@@ -46,12 +48,12 @@
     dataOut[4] = s2;
     //pc.printf("1:%f %d\r\n", f1, dataOut[3]);
     //pc.printf("2:%f %d\r\n", f2, dataOut[4]);
-    //dataOut[2]=fsm.error();
+    dataOut[2]=fsm.error();
     short* ptr=dataIn;
     ptr=sendData(dataOut, 9, dataIn);
-    if (dataIn[1] != 0) {   
+    /*if (dataIn[1] != 0) {   
         pc.printf("UI: %d\r\n", dataIn[1]);
-    }
+    }*/
     //Sends message received from the ctrlbed to the dataComm object
     dc.process_write(dataIn+2, 7);
     //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
@@ -59,7 +61,7 @@
     // Run state change/analysis in FSM
     int exoState=fsm.state(dataIn[1]);
     float temp=dbg.read_us();
-    //pc.printf("%f\r\n",temp);
+    pc.printf("%f\r\n", temp);
     
 }
 
@@ -67,8 +69,9 @@
 int main()
 {
     pc.printf("\r\nExoStart \r\n");
+    wait(1);
     initializeExoIOs();
-    //pc.printf("Test\r\n"); // keep for debugging compile errors
+    pc.printf("Test\r\n"); // keep for debugging compile errors
     mbedLED1 = 1;
     pc.printf("Starting exo...\n\r");
     //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