Diff: main.cpp

Revision:

20:cf8e99d989a6

Parent:

19:70b2124c7370

Child:

21:715c8cf78e14

--- a/main.cpp	Fri Mar 20 23:09:02 2015 +0000
+++ b/main.cpp	Mon Apr 13 17:44:21 2015 +0000
@@ -27,14 +27,24 @@
 void periodicFcns()
 {
     dbg.reset();
-    //float temp1 = dbg.read_us();
-   
-    //dbg.start();
-    dataOut[1]=encoder_L.readRaw();
+//    dataOut[1]=encoder_L.readRaw();
+    float f1 = encoder_L.read_angle();
+    float f2 = fsm.read_angle_y();
+    dataOut[1] = (short)f1;
+    dataOut[2] = (short)f1;
+    dataOut[3] = (short)f1;
+    dataOut[4] = (short)f2;
+    dataOut[5] = (short)f2;
+    //pc.printf("%f\r\n", f1);
+    pc.printf("%d\r\n", (short)f1);
+    //pc.printf("%f\r\n", f2);
+    pc.printf("%d\r\n", (short)f2);
+    //for (int i = 2; i < 4; i++) {
+      //  pc.printf("data%d is %x\r\n", i, dataOut[i]);
+    //}
     //dataOut[2]=fsm.error();
     short* ptr=dataIn;
     ptr=sendData(dataOut, 7, dataIn);
-    //dbg.reset();
     /*for (int i = 0; i < 7; i++) {   
         pc.printf("%x, ", dataIn[i]);
     }
@@ -45,18 +55,18 @@
     // 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);
     
 }
 
+//Starts the Exo controlbed, and processes messages by repeatedly executing periodicFcns
 int main()
 {
     pc.printf("\r\nExoStart \r\n");
     initializeExoIOs();
     //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
     wait(2);
     dbg.start();