Diff: BluetoothComm.cpp

Revision:

16:b6b2c7b0f1c9

Parent:

15:a492356f5485

Child:

17:80affee96096

--- a/BluetoothComm.cpp	Fri Mar 20 20:27:39 2015 +0000
+++ b/BluetoothComm.cpp	Mon Apr 13 17:40:41 2015 +0000
@@ -4,20 +4,13 @@
 #include "initDatabed.h"
 #include <string>
 #include <map>
-//Variable indices for set_values: set_values should take an array of length NUMVARS + NUMREADONLYPARAMS = 34. 
-//Map of variables to array indices is as follows:
-//0 = Torso Angle, 1 = Left Knee Angle, 2 = Right Knee Angle, 3 = Left Hip angle, 4 = Right Hip angle, 5 = Left Hip Torque, 6 = Right Hip Torque
-//8 = Exo State/Left Knee State/Right Knee State 9 = Torso Ref. Angle, 10 = Left Hip Ref. Angle, 11 = Right Hip Ref. Angle
-//12 = KP Stance, 13 = KP Swing, 14 = KP Standing, 15 = KP Sitting, 16 = KP Standing up, 17 = KP Sitting down
-//18 = KD Stance, 19 = KD Swing, 20 = KD Standing, 21 = KD Sitting, 22 = KD Standing up, 23 = KD sitting down
-//24 = Standing angle, 25 = Sitting angle, 26 = Bent Forward Angle, 27 = Forward Angle, 28 = Rear Angle, 29 = IMU Angle
-//30 = Knee Full Retract, 31 = Knee Full Extend, 32 = Lock Time, 33 = Rate
+
 BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(27), _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);
-    //pc.printf("Started BTComm init \r\n");
+    
     _rn42.baud(115200);
     int temp1[] = {0,1,2,3,4,8,9,10, -1};
   
@@ -37,20 +30,19 @@
     }
     
  
-    write_params_to_sd_card();
-  //  pc.printf("Initialized PARAM \r\n");
+//    write_params_to_sd_card();
+
     int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
-    readData.write(_numReadOnlyParams, temp4);
+    //readData.write(_numReadOnlyParams, temp4);
     for (int k = 0; k < _numReadOnlyParams; k += 1) {
         _paramMap[_indexMap[_numVars + k]] = temp4[k];
     }
-    //printf("Test: %x\r\n", _paramMap["TorsoAng"]);
-    write_data_to_sd_card();
+    
+  //  write_data_to_sd_card();
     //Fill the parameter map with data from SD card
-    read_data_from_sd();
-    //pc.printf("Read data\r\n");
-    //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]);
-    read_params_from_sd();
+   // read_data_from_sd();
+    
+    //read_params_from_sd();
     pc.printf("Finished BTComm init\r\n");
 
 }
@@ -60,30 +52,32 @@
 * @author Michael Ling
 * @date 2/4/2015
 */
+/**
 void BluetoothComm::read_data_from_sd()
 {
     
     int *arr = readData.read(_numReadOnlyParams, arr);
     for (int i = 0; i < _numReadOnlyParams; i += 1) {
         _paramMap[_indexMap[i + _numVars]] = arr[i];
-       // printf("Read: %x\r\n", arr[i]);
+       
     }
-  //  printf("Finished reading data\r\n");
-}
+  
+}*/
 
 /**
 * 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);
     for (int i = 0; i < _numVars; i += 1) {
         _paramMap[_indexMap[i]] = arr[i];
     }
-    //printf("Finished reading params\r\n");
-}   
+    
+}*/
 
 /**
 * Calculates parity--0 if c even, 1 if odd
@@ -143,13 +137,7 @@
     _rn42.putc(0);
     _rn42.putc(0);
 }
-/**
-//Sets charge level. DEPRECATED
-void BluetoothComm::setCharge(short level) {
-   // _readOnlyParams[11] = level;
-   _paramMap["Charge"] = level;
-}
-*/
+
 
 /**
 * Sends the specified char array through the _rn42 Bluetooth connection.
@@ -183,6 +171,7 @@
 * @author Michael Ling
 * @date 2/4/2015
 */
+/**
 void BluetoothComm::write_params_to_sd_card()
 {
   
@@ -192,13 +181,14 @@
     }
     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];
@@ -207,11 +197,13 @@
     //    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
 * @param paramList    List of parameters to be sent over Bluetooth, represented as a char array
+* @param message      The message we want to send to Ctrlbed
+* @param dataOut      Circular buffer that allow communication with Ctrlbed     
 * @author Michael Ling
 * @date 2/4/2015
 */
@@ -247,29 +239,33 @@
     msg[len+1] = checksum[1];
     msg[len+2] = checksum[2];
     len += 3;
+    //This loop sends a reply to the phone
     for (int j = 0; j < len; j++) {
         printf("Sending char %x \r\n", msg[j]);
         _rn42.putc(msg[j]);
         
     }
+    //This loop passes the original write message to the ctrlbed
+    //Each pair of characters is converted to a short, with orders reversed. 
+    //This is so that casting the short* back to char* restores the original message.
     if (dataOut != NULL) {
         int ind = 0;
         int j;
         for (j = 0; j < len-1; j+=2) {
             dataOut[ind] = (message[j+1]<<8)|message[j];
-            //printf("Index %d is %x\r\n", ind, dataOut[ind]);
+            
             ind += 1;
         }
+        //If the message has an odd number of characters
         if (j < len) {
             dataOut[ind] = message[j];
-            //printf("Index %d is %x\r\n", ind, dataOut[ind]);
+            
         } 
         for (int k=0; k<len;k++) {
             printf("Index %d: %x\r\n", k, dataOut[k]);
         }
         
     }
-  //  printf("Placed in dataOut\r\n");
     memcpy(_lastCmd, msg, 50);
     free(checksum);
     return ;
@@ -282,30 +278,19 @@
 */
 void BluetoothComm::send_read_only_values()
 {
-    //printf("SENDING RO VALUES!\r\n");
+   
     int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7;
- //   printf("msglen is %d\r\n", msgLen);
     char message[msgLen];
-   // printf("message of len 39 created\r\n");
     message[0] = START;
-    //printf("%x\r\n", message[0]);
     message[1] = 0;
-    //printf("%x\r\n", message[1]);
     message[2] = READONLY_IND;
-    //printf("3 bytes of message set\r\n");
     int msgInd = 3;
     int escapes = 0;
-    //printf("%d readonly parameters", _numReadOnlyParams);
     for (int i = 0; i < _numReadOnlyParams; i++) {
         if (i == _escapeNeeded[escapes]) {
-           // printf("Escape char. needed at index %d \r\n", i);
-            //char conflict = (char)(_readOnlyParams[i] & 0xff);
             char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
-            //printf("%x possibly has a conflict in %x \r\n", _readOnlyParams[i], conflict);
             escapes += 1;
-            //message[msgInd+1] = (char) (_readOnlyParams[i] >> 8);
             message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
-          //  printf("Set msgInd+1 to %x \r\n", message[msgInd+1]);
             if (conflict == (char) 0xfe) {
                 message[msgInd] = 1;
                 message[msgInd+2] = 0xfc;
@@ -318,8 +303,6 @@
             }
             msgInd += 3;
         } else {
-      //      message[msgInd] = (char) (_readOnlyParams[i] >> 8);
-        //    message[msgInd+1] = (char) (_readOnlyParams[i] & 0xff);
             message[msgInd] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
             message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] & 0xff);
             msgInd += 2;
@@ -330,14 +313,11 @@
     message[msgLen-3] = checksum[0];
     message[msgLen-2] = checksum[1];
     message[msgLen-1] = checksum[2];
-    //printf("Sending the following readONly values: \r\n");
     for (int j=0; j < msgLen; j++) {
-      //  printf("%x \r\n", message[j]);
         _rn42.putc(message[j]);
     }
     memcpy(_lastCmd, message, 50);
     free(checksum);
-    //printf("Finished sending readOnly values \r\n");
 
 }
 
@@ -351,7 +331,6 @@
 bool BluetoothComm::msg_check(char* message, int len)
 {
     if (message[0] != START) {
-        //printf("Improper START or END \r\n");
         send_error(START_ERR);
         return false;
     }
@@ -368,12 +347,10 @@
             break;
         }
         if (((message[i] & 0x80) !=0) && !write) {
-            //printf("Does not match READ format \r\n");
             send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
             return false;
         }
         if (((message[i] & 0x80) != 0x80) && write) {
-            //printf("char %x Does not match WRITE format \r\n", message[i]);
             send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
             return false;
         }
@@ -386,7 +363,6 @@
         bool parity1 = (s & 0x4000) != 0;
 
         if (parity1 != parity(s & 0x4000)) {
-            //printf("Parity error in VAR char \r\n");
             send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
             return false;
         }
@@ -394,31 +370,26 @@
         char c = message[i] & 0x3f;
         char c2 = message[i+1];
         if ((int) c < 0 || (int) c > _numVars) {
-            //printf("VAR char out of range \r\n");
             send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
             return false;
         }
         if ((int) c2 < 0 || (int) c2 > 100) {
-            //printf("DATA char out of range");
             send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
             return false;
         }
     }
     char* checksum = get_checksum(message, len-3);
     if (checksum[0] != message[len-3]) {
-        //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
         free(checksum);
         send_error(CHECKSUM_ERR);
         return false;
     }
     if (checksum[1] != message[len-2]) {
-        //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
         free(checksum);
         send_error(CHECKSUM_ERR);
         return false;
     }
     if (checksum[2] != message[len-1]) {
-        //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
         free(checksum);
         send_error(CHECKSUM_ERR);
         return false;
@@ -437,15 +408,13 @@
 */
 void BluetoothComm::process_read_only(char* message, int len)
 {
-    //printf("Message is a ReadOnly \r\n");
+   
     if (!msg_check(message, len)) {
         printf("msg_check failed on readonly! \r\n");
         return;
     }
-    //PASS TO CTRLBED
     _failures = 0;
 
-    //printf("Sending readOnly values \r\n");
     send_read_only_values();
 }
 
@@ -461,7 +430,6 @@
     //If the received message is an error message, resend the last command
     if (message[2] == END) {
         _failures += 1;
-        //printf("_failures: %d\r\n", _failures);
         if (_failures < 5) {
             send(_lastCmd);
         } else {
@@ -470,32 +438,26 @@
         return;
     }
     if (!msg_check(message, len)) {
-        //printf("msg_check failed on read! \r\n");
         return;
     }
     //PASS MSG TO CTRLBED
     _failures = 0;
-    //printf("Message is a read \r\n");
     char paramList[_numVars];
     memset(paramList, 0xff, _numVars);
 
     for (int i=2; i < len-5; i++) {
         char msg = message[i] & 0xbf;
         if ((msg & 0x80) != 0) {
-       // printf("Got a non-read char %x...exiting \r\n", msg);
             return;
         }
 
         int index = msg & 0xff;
-        //printf("Value at index %d requested \r\n", index);
         paramList[index] = _paramMap[_indexMap[index]];
 
     }
     if (message[len-5] == READONLY_IND) {
-       // printf("Need to send RO vals\r\n");
         send_read_only_values();
     }
- //   printf("About to send PARAMLIST\r\n");
     send_values(paramList, NULL, NULL);
 }
 
@@ -503,6 +465,7 @@
 * Checks received WRITE message and writes to paramMap/SDCard
 * @param message    The received WRITE message
 * @param len    Length of the WRITE message
+* @param dataOut Buffer that contains message for Ctrlbed
 * @author Michael Ling
 * @date 2/4/2015
 */
@@ -522,10 +485,10 @@
         printf("msg_check failed on write! \r\n");
         return;
     }
-    //PASS msg. to ControlBed
+   
     char paramList[_numVars];
     memset(paramList, 0xff, _numVars);
-    //printf("Message is a write \r\n");
+    //Strips received message of parity and write bits, sets paramMap to the new values, then passes the phone/ctrlbed messages to sendValues
     for (int i=2; i < len-5; i+=2) {
         char msg = message[i] & 0xbf;
         if ((msg & 0x80) != 0x80) {
@@ -539,15 +502,15 @@
     }
     send_values(paramList, message, dataOut);
     
-    write_params_to_sd_card();
-    //SD_Card.write(localValues)
-    //sendToControlBed(localValues+1, len-2)
+   // write_params_to_sd_card();
+    
 }
 
 /** 
 * Checks if received message is a read, write or readonly
 * @param message    The received message
 * @param len    Length of the message
+* @param dataOut Buffer for sending to ctrlbed
 * @author Michael Ling
 * @date 2/4/2015
 */
@@ -574,6 +537,7 @@
 //Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
 /**
 * Scans for data received through Bluetooth, and passes it on if it detects a message-like chunk. Should be run via an interuupt.
+* @param dataOut    Buffer for sending to Ctrlbed
 * @author Michael Ling
 * @date 2/4/2015
 */
@@ -592,14 +556,12 @@
         }
         //This marks the START of a message
         if (_inMsg == false and b == START) {
-            boardLed4 = 1;
-        //    printf("Msg START received \r\n");
+            //boardLed4 = 1;
             _inMsg = true;
             _counter = 3;
             _curMsg[_len] = b;
             _len += 1;
         } else if (_inMsg == true and b == START) {
-            //     printf("Second start received, terminating\r\n");
             _inMsg = false;
             _counter = 0;
             memset(_curMsg, 0, 50);
@@ -607,7 +569,6 @@
             process(_msg, _len, dataOut);
             _len = 0;
         } else if (_inMsg || _counter > 0 ) {
-            //   printf("_inMsg or _counter > 0 \r\n");
             _curMsg[_len] = b;
             _len += 1;
             if (!_inMsg) {
@@ -615,7 +576,6 @@
             }
             //Marks end of message, and starts processing
             if (_counter <= 0) {
-                //         printf("End of message \r\n");
                 memset(_msg, 0, 50);
                 memcpy(_msg, _curMsg, 50);
                 memset(_curMsg, 0, 50);
@@ -627,9 +587,9 @@
         if (b == END) {
             _inMsg = false;
 
-            // _rn42.putc(msg);
+           
         }
-        //_rn42.putc(_data);
+        
           
     }
 }