Diff: main.cpp

Revision:

24:2e56d3bebb24

Parent:

23:e0923403be2f

Child:

25:1292b886b8d2

--- a/main.cpp	Mon Apr 27 21:57:54 2015 +0000
+++ b/main.cpp	Wed May 06 17:14:00 2015 +0000
@@ -16,6 +16,8 @@
 #include "dataBedComm.h"
 #include "dataComm.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 */
 short dataOut[]={0xFF,30,31,0, 0, 0, 0, 0, 0xFE};
 dataComm dc = dataComm();
 Timer dbg;
@@ -25,43 +27,30 @@
 // periodicFcns runs at the start of every control loop cycle
 // It initiates communication with dataBed, checks for errors/safety, and starts the FSM
 
-union float_to_short {
-    float f;
-    uint32_t u;
-} q;
+
+
 void periodicFcns()
 {
     dbg.reset();
-//    dataOut[1]=encoder_L.readRaw();
+    //dataOut[1]=encoder_L.readRaw();
     float f1 = encoder_L.read_angle();
     float f2 = fsm.read_angle_y();
-    //float f2 = -87.43423;
+  
+    /** Multiplying by 91 allows casting float to short without losing too much precision
+    * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow
+    */
     short s1 = (short)(f1*91);
     short s2 = (short)(f2*91);
-    dataOut[1] = s1;
-    dataOut[2] = s2;
-    pc.printf("first is %f %d\r\n", f1, s1);
-    pc.printf("second is %f %d\r\n", f2, s2);
-    //dataOut[3] = (short)f1;
-    //dataOut[4] = (short)f2;
-    //dataOut[5] = (short)f2;
-    //pc.printf("%f\r\n", f1);
-    //pc.printf("data1: %d\r\n", dataOut[1]);
-    //pc.printf("%f\r\n", f2);
-    //pc.printf("Size %d\r\n", sizeof dataOut[2]);
-    //pc.printf("data2: %d\r\n", dataOut[2]);
-    //for (int i = 2; i < 4; i++) {
-      //  pc.printf("data%d is %x\r\n", i, dataOut[i]);
-    //}
+    dataOut[3] = s1;
+    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();
     short* ptr=dataIn;
     ptr=sendData(dataOut, 9, dataIn);
-    //for (int i = 0; i < 7; i++) {
-        if (dataIn[1] != 0) {   
-    pc.printf("UI: %d\r\n", dataIn[1]);
+    if (dataIn[1] != 0) {   
+        pc.printf("UI: %d\r\n", dataIn[1]);
     }
-    //}
-    //pc.printf("\r\n");*/
     dc.process_write(dataIn+2, 7);
     //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);