Diff: BluetoothComm.cpp

Revision:

20:be8dcc2344bc

Parent:

19:f3eece6c024f

--- a/BluetoothComm.cpp	Mon Apr 27 21:58:27 2015 +0000
+++ b/BluetoothComm.cpp	Mon May 11 21:47:40 2015 +0000
@@ -5,15 +5,18 @@
 #include <string>
 #include <map>
 
-BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8)
+BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8)
 {
     
     /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */
     //_rn42.baud(9600);
     
     _rn42.baud(115200);
+    
+    //Readonly indices where an escape character may be needed, terminated by a -1
     int temp1[] = {0,1,2,3,4,8,9,10, -1};
   
+    //Sets the _escapeNeeded array with the above indices
     for (int i = 0; i < _escapesNeeded+1; i++) {
         _escapeNeeded[i] = temp1[i];
     }
@@ -22,7 +25,7 @@
     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",
-        "StepLength", "StepTime", "MaxAmplitude", "StanceStart", "StanceEnd", 
+        "StepLength", "StepTime", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd", 
         "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
         "RHipRefAngle", "Charge"};
     //Populate the map of indices to param names
@@ -30,9 +33,11 @@
         _indexMap[j] = temp2[j];
     }
     
- 
+    
+    //Will overwrite the parameter files with all 0's--REMOVE when we figure out how to retain SDCard data
     write_params_to_sd_card();
-
+    
+    //Dummy data to initialize readonly values, remove later...
     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) {
@@ -40,15 +45,11 @@
     }
     //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();
-  //  pc.printf("Data is read\r\n");
+  
     read_params_from_sd();
     pc.printf("Finished BTComm init\r\n");
 
@@ -84,7 +85,8 @@
 
 void BluetoothComm::read_params_from_sd()
 {
-    int *arr = param.read(_numVars, arr);
+    int arr[_numVars];
+    param.read(_numVars, arr);
     for (int i = 0; i < _numVars; i += 1) {
         _paramMap[_indexMap[i]] = arr[i];
     }
@@ -162,7 +164,7 @@
     for (int i = 0; i < 50; i++) {
         _rn42.putc(cmd[i]);
     }
-}
+}                 
 
 /**
 * Sets the parameter map, based on the input map NEWVALUES
@@ -178,6 +180,15 @@
     }
 }
 
+
+
+/**
+* Changes the value of a single param in the SD card
+* @param index    Index to be set
+* @param value    New value for that param
+* @author Michael Ling
+* @date 5/11/2015
+*/
 void BluetoothComm::set_data(int index, short value) {
     //pc.printf("New value for %d\r\n", index);
     readData.write_to_index(index, (int)value);
@@ -233,10 +244,11 @@
     msg[0] = START;
     msg[1] = 0;
     for (int i=0; i < _numVars; i++) {
-        
+        //If paramList[i] is not 0xff, that means it was set by the last write
         if (paramList[i] != 0xff) {
             short s = (short)((i << 8) | paramList[i]);
             //printf("In send_values, calculating parity of %x\r\n", s);
+            //This block adds parity bit, if needed
             if (parity(s)) {
                 //printf("%x requires TRUE parity bit\r\n", s);
                 msg[len] = (char)(i | 0x40);
@@ -252,6 +264,7 @@
     }
     msg[len] = END;
     len += 1;
+    //Computes checksum and appends it to the message
     char* checksum = get_checksum(msg, len);
     msg[len] = checksum[0];
     msg[len+1] = checksum[1];
@@ -308,6 +321,10 @@
     int escapes = 0;
     for (int i = 0; i < _numReadOnlyParams; i++) {
         if (i == _escapeNeeded[escapes]) {
+            //Format of the message is one escape byte (if in escapeNeeded) plus two data bytes
+            //Before putting a readonly value into the message, check if its bytes match 0xfe or 0xff
+            //If a conflict is detected, replace the offending byte with 0xfc and 0xfd, respectively...
+            //...and set either the rightmost or 2nd-rightmost bit of the escape byte, depending on that byte's position
             char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
             escapes += 1;
             //message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
@@ -339,6 +356,7 @@
             msgInd += 2;
         }
     }
+    //Readonly message need a checksum as well
     message[msgLen-4] = END;
     char* checksum = get_checksum(message, msgLen-3);
     message[msgLen-3] = checksum[0];
@@ -361,6 +379,7 @@
 */
 bool BluetoothComm::msg_check(char* message, int len)
 {
+    //Message must have a START and an END
     if (message[0] != START) {
         send_error(START_ERR);
         return false;
@@ -371,12 +390,14 @@
     }
     bool write = message[2] & 0x80;
     for (int i=2; i < len-5; i+=2) {
+        //This block indicates a purely readonly message
         if (i == 2 && message[i] == READONLY_IND) {
             break;
         }
         if (i == (len-5) && message[i] == READONLY_IND) {
             break;
         }
+        //Error if the write bit is set in a read message, or not set in a write message
         if (((message[i] & 0x80) !=0) && !write) {
             send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
             return false;
@@ -393,6 +414,7 @@
         }
         bool parity1 = (s & 0x4000) != 0;
 
+        //Error if the two-byte chunk has wrong parity
         if (parity1 != parity(s & 0x4000)) {
             send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
             return false;
@@ -400,15 +422,18 @@
 
         char c = message[i] & 0x3f;
         char c2 = message[i+1];
+        //Error if the param index is out of range
         if ((int) c < 0 || (int) c > _numVars) {
             send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
             return false;
         }
+        //Error if the new param value is out of range
         if ((int) c2 < 0 || (int) c2 > 100) {
             send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
             return false;
         }
     }
+    //Error if checksum calculated incorrectly
     char* checksum = get_checksum(message, len-3);
     if (checksum[0] != message[len-3]) {
         free(checksum);
@@ -459,6 +484,7 @@
 void BluetoothComm::process_read(char* message, int len)
 {
     //If the received message is an error message, resend the last command
+    read_params_from_sd();
     if (message[2] == END) {
         _failures += 1;
         if (_failures < 5) {
@@ -471,7 +497,7 @@
     if (!msg_check(message, len)) {
         return;
     }
-    //PASS MSG TO CTRLBED
+    
     _failures = 0;
     char paramList[_numVars];
     memset(paramList, 0xff, _numVars);
@@ -486,9 +512,11 @@
         paramList[index] = _paramMap[_indexMap[index]];
 
     }
+    //presence of READONLY_IND at the end of the indices to read indicates that we want to read readonly values as well as editable params
     if (message[len-5] == READONLY_IND) {
         send_read_only_values();
     }
+    //read_params_from_sd();
     send_values(paramList, NULL, NULL);
 }
 
@@ -558,6 +586,7 @@
     if ((c & 0x80) == 0) {
         //printf("Is a read\r\n");
         process_read(message, len);
+        param.debug_print();
         return;
     } else {
         process_write(message, len, dataOut);
@@ -592,6 +621,7 @@
             _counter = 3;
             _curMsg[_len] = b;
             _len += 1;
+        //Kill the current message if we receive a second START
         } else if (_inMsg == true and b == START) {
             _inMsg = false;
             _counter = 0;
@@ -599,9 +629,11 @@
             _rn42.rxBufferFlush();
             process(_msg, _len, dataOut);
             _len = 0;
+        //Read the received byte into the message, if we're still in a message...
         } else if (_inMsg || _counter > 0 ) {
             _curMsg[_len] = b;
             _len += 1;
+            //This will happen if we're processing the checksum--we want to read 3 bytes past the END
             if (!_inMsg) {
                 _counter -= 1;
             }