Diff: BluetoothComm.cpp

Revision:

18:34ccf02fdbe7

Parent:

17:80affee96096

Child:

19:f3eece6c024f

--- a/BluetoothComm.cpp	Wed Apr 15 01:50:50 2015 +0000
+++ b/BluetoothComm.cpp	Fri Apr 24 19:32:36 2015 +0000
@@ -5,7 +5,7 @@
 #include <string>
 #include <map>
 
-BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(27), _numReadOnlyParams(12), _escapesNeeded(8)
+BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8)
 {
     
     /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */
@@ -21,7 +21,8 @@
   
     std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
         "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
-        "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle", 
+        "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
+        "StepLength", "StepTime", "PeakFlexion", "PhaseShift", "WalkSpeed", 
         "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
         "RHipRefAngle", "Charge"};
     //Populate the map of indices to param names
@@ -30,19 +31,25 @@
     }
     
  
-//    write_params_to_sd_card();
+    write_params_to_sd_card();
 
     int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
     //readData.write(_numReadOnlyParams, temp4);
     for (int k = 0; k < _numReadOnlyParams; k += 1) {
         _paramMap[_indexMap[_numVars + k]] = temp4[k];
     }
-    
-  //  write_data_to_sd_card();
+    //pc.printf("Starting sdcard stuff\r\n");
+    write_data_to_sd_card();
+    //read_data_from_sd();
+   // set_data(0, 0xdead);
+    //set_data(1, 0xdead);
+    //set_data(2, 0xdead);
+    //set_data(3, 0xdead);
+   // pc.printf("Data is written\r\n");
     //Fill the parameter map with data from SD card
-   // read_data_from_sd();
-    
-    //read_params_from_sd();
+    read_data_from_sd();
+  //  pc.printf("Data is read\r\n");
+    read_params_from_sd();
     pc.printf("Finished BTComm init\r\n");
 
 }
@@ -52,24 +59,29 @@
 * @author Michael Ling
 * @date 2/4/2015
 */
-/**
+
+
 void BluetoothComm::read_data_from_sd()
 {
-    
-    int *arr = readData.read(_numReadOnlyParams, arr);
+    pc.printf("Beginning to read\r\n");
+    int arr[_numReadOnlyParams];
+    readData.read(_numReadOnlyParams, arr);
+    pc.printf("Finished read\r\n");
     for (int i = 0; i < _numReadOnlyParams; i += 1) {
+        //pc.printf("arr[%d] is %x\r\n", i, arr[i]);
         _paramMap[_indexMap[i + _numVars]] = arr[i];
        
     }
+    
   
-}*/
+}
 
 /**
 * Reads editable parameters from SD card, and fills in the parameter map
 * @author Michael Ling
 * @date 2/4/2015
 */
-/**
+
 void BluetoothComm::read_params_from_sd()
 {
     int *arr = param.read(_numVars, arr);
@@ -77,7 +89,7 @@
         _paramMap[_indexMap[i]] = arr[i];
     }
     
-}*/
+}
 
 /**
 * Calculates parity--0 if c even, 1 if odd
@@ -166,12 +178,18 @@
     }
 }
 
+void BluetoothComm::set_data(int index, short value) {
+    //pc.printf("New value for %d\r\n", index);
+    readData.write_to_index(index, (int)value);
+    
+}
+
 /**
 * Writes the editable params. stored in _paramMap to the SD card
 * @author Michael Ling
 * @date 2/4/2015
 */
-/**
+
 void BluetoothComm::write_params_to_sd_card()
 {
   
@@ -181,23 +199,23 @@
     }
     param.write(_numVars, paramValues);
     
-}*/
+}
 
 /**
 * Write the read-only values stored in _paramMap to the SD card
 * @author Michael Ling
 * @date 2/4/2015
 */
-/**
+
 void BluetoothComm::write_data_to_sd_card()
 {
     int dataValues[_numReadOnlyParams];
     for (int i = 0; i < _numReadOnlyParams; i += 1) {
         dataValues[i] = (int) _paramMap[_indexMap[i + _numVars]];
-    //    printf("Index %d of dataValues set to %x\r\n", i, dataValues[i]);
+        //pc.printf("Index %d of dataValues set to %x\r\n", i, dataValues[i]);
     }
     readData.write(_numReadOnlyParams, dataValues);
-}*/
+}
 
 /**
 * Sends the paramList with START/END, parity bits, and a checksum
@@ -278,7 +296,9 @@
 */
 void BluetoothComm::send_read_only_values()
 {
-   
+    pc.printf("Send read only\r\n");
+    read_data_from_sd();
+    pc.printf("Updated readonly map\r\n");
     int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7;
     char message[msgLen];
     message[0] = START;
@@ -290,15 +310,26 @@
         if (i == _escapeNeeded[escapes]) {
             char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
             escapes += 1;
-            message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
+            //message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
+            char conflict2 = (char)(_paramMap[_indexMap[i+_numVars]] >> 8);
+            if (conflict2 == (char) 0xfe) {
+                message[msgInd] = 2;
+                message[msgInd+1] = 0xfc;
+            } else if (conflict2 == (char) 0xff) {
+                message[msgInd] = 2;
+                message[msgInd+1] = 0xfd;
+            } else {
+                message[msgInd] = 0;
+                message[msgInd+1] = conflict2;
+            }
             if (conflict == (char) 0xfe) {
-                message[msgInd] = 1;
+                message[msgInd] |= 1;
                 message[msgInd+2] = 0xfc;
             } else if (conflict == (char) 0xff) {
-                message[msgInd] = 1;
+                message[msgInd] |= 1;
                 message[msgInd+2] = 0xfd;
             } else {
-                message[msgInd] = 0;
+                //message[msgInd] = 0;
                 message[msgInd+2] = conflict;
             }
             msgInd += 3;
@@ -502,7 +533,7 @@
     }
     send_values(paramList, message, dataOut);
     
-   // write_params_to_sd_card();
+    write_params_to_sd_card();
     
 }