Diff: dataComm.cpp

Revision:

4:86d9e33652b6

Parent:

3:2091104c9b61

Child:

5:c95a0006ba68

--- a/dataComm.cpp	Fri Apr 24 19:34:49 2015 +0000
+++ b/dataComm.cpp	Mon Apr 27 21:57:35 2015 +0000
@@ -1,5 +1,5 @@
 #include "mbed.h"
-//#include "MODSERIAL.h"
+
 #include "dataComm.h"
 #include "initExoVars.h"
 #include "gaitGenerator.h"
@@ -8,40 +8,37 @@
 
 dataComm::dataComm():  _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8)
 {
-    
-    
-    int temp1[] = {0,1,2,3,4,8,9,10, -1};
-  
+
+    int temp1[] = {0,1,2,3,4,8,9,10,-1};
+
     for (int i = 0; i < _escapesNeeded+1; i++) {
         _escapeNeeded[i] = temp1[i];
     }
-    
-  
+
+
     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", "PeakFlexion", "PhaseShift", "WalkSpeed",
+        "StepLength", "StepTime", "MaxAmplitude", "StanceStart", "StanceEnd",
         "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
         "RHipRefAngle", "Charge"};
     //Populate the map of indices to param names
     for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
         _indexMap[j] = temp2[j];
     }
-    
+
     for (int j = 0; j < _numVars; j += 1) {
         _paramMap[_indexMap[j]] = generic_get(_indexMap[j]);
     }
- 
+
   //  pc.printf("Initialized PARAM \r\n");
     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];
     }
     //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]);
 
- 
-
 }
 
 /**
@@ -52,23 +49,25 @@
 */
 short dataComm::generic_get(std::string var)
 {
-    if (var.compare("SittingAngle") == 0){
+    if (var.compare("SittingAngle") == 0) {
         return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100);
-    }else if(var.compare( "BentForwardAngle")==0){
+    } else if(var.compare( "BentForwardAngle")==0) {
         return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100);
-    } else if (var.compare("StandupAsst")==0){
+    } else if (var.compare("StandupAsst")==0) {
         return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100);
     } else if (var.compare("SitdownAsst")==0) {
         return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100);
-    } else if (var.compare("SitdownTime")==0){
+    } else if (var.compare("SitdownTime")==0) {
         return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100);
-    } else if (var.compare( "StandingAngle")==0){
+    } else if (var.compare( "StandingAngle")==0) {
         return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100);
-    } else if (var.compare("WalkAngle")==0){
+    } else if (var.compare("WalkAngle")==0) {
         return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100);
+    } else if (var.compare("StepTime")==0) {
+        return (short) step_time;
     }
     return 0;
- 
+
 }
 
 /**
@@ -80,37 +79,39 @@
 */
 void dataComm::generic_set(std::string var, short newval)
 {
-        mbedLED3 = 1;
-        //pc.printf("%s\r\n", var);
-        //newval is a short from 0-100, and needs to be converted a more
-        if (var.compare( "SittingAngle")==0){
-            sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT);
-         //   pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100));
-        } else if (var.compare( "BentForwardAngle")==0){
-            
-            bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT);
-           // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100));
-        } else if (var.compare("StandupAsst")==0) {
-            fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST));
-          //  pc.printf("%d\r\n", (short)fsm.get_standup_asst());
-        } else if (var.compare("SitdownAsst")==0) {
-            fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST));
-          //  pc.printf("%d\r\n", (short)fsm.get_sitdown_asst());
-        } else if (var.compare("SitdownTime")==0) {
-            tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME);
-           // pc.printf("%d\r\n", (short)tSittingDown);
-        } else if (var.compare("StandingAngle")==0) {
-            pc.printf("LED\r\n");
-            motorLED = 1;
-            stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND);
-            encoder_L.init(zero_ang_L+stand_adjust);
-            encoder_R.init(zero_ang_R+stand_adjust);
-            //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100));
-         } else if (var.compare("WalkAngle")==0) {
-            fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK));
-           // pc.printf("%d\r\n", (short)fsm.get_backbias());
-        }
-        
+    mbedLED3 = 1;
+    //pc.printf("%s\r\n", var);
+    //newval is a short from 0-100, and needs to be converted a more
+    if (var.compare( "SittingAngle")==0) {
+        sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT);
+        //   pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100));
+    } else if (var.compare( "BentForwardAngle")==0) {
+        bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT);
+       // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100));
+    } else if (var.compare("StandupAsst")==0) {
+        fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST));
+        //  pc.printf("%d\r\n", (short)fsm.get_standup_asst());
+    } else if (var.compare("SitdownAsst")==0) {
+        fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST));
+        //  pc.printf("%d\r\n", (short)fsm.get_sitdown_asst());
+    } else if (var.compare("SitdownTime")==0) {
+        tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME);
+        // pc.printf("%d\r\n", (short)tSittingDown);
+    } else if (var.compare("StandingAngle")==0) {
+        pc.printf("LED\r\n");
+        motorLED = 1;
+        stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND);
+        encoder_L.init(zero_ang_L+stand_adjust);
+        encoder_R.init(zero_ang_R+stand_adjust);
+        //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100));
+    } else if (var.compare("WalkAngle")==0) {
+        fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK));
+       // pc.printf("%d\r\n", (short)fsm.get_backbias());
+    } else if (var.compare("StepTime")==0) {
+        step_time = (float)newval;
+    }
+    //MORE else blocks for gait params
+
 }
 /**
 * Calculates parity--0 if c even, 1 if odd
@@ -155,38 +156,7 @@
     return checksum;
 }
 
-/**
-* Sends error message.
-* @param errCode    character representation of the failure-causing error
-* @author Michael Ling
-* @data 2/4/2015
 
-void dataComm::send_error(char errCode)
-{
-    _rn42.putc(START);
-    _rn42.putc(errCode);
-    _rn42.putc(END);
-    _rn42.putc(0);
-    _rn42.putc(0);
-    _rn42.putc(0);
-}
-*/
-
-
-
-
-
-
-
-char* dataComm::convert_to_char_array(short int* message, int len) {
-    char *retval = new char[len*2];
-    for (int i = 2; i < len; i+=2) {
-        short int val = message[i];
-        retval[(i-1)*2] = (val >> 8) & 0xff;
-        retval[(i-1)*2+1] = val & 0xff;
-    }
-    return retval;
-}
 
 /**
 * Checks the message with length len. Checks for START and END in correct spots, parity, all data and variable byte in proper range, and correct checksum
@@ -200,7 +170,7 @@
     //Checks the message starts with 0xff
     if (message[0] != START) {
         //printf("Improper START or END \r\n");
-        //send_error(START_ERR);
+
         return false;
     }
     //printf("got a start\r\n");
@@ -222,12 +192,10 @@
         }
         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;
         }
         short s;
@@ -241,7 +209,6 @@
 
         if (parity1 != parity(s & 0x4000)) {
             //printf("Parity error in VAR char \r\n");
-            //send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
             return false;
         }
 
@@ -250,13 +217,11 @@
         //Makes sure the var is a valid one
         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;
         }
         //Makes sure the new value is in the range 0-100
         if ((int) c2 < 0 || (int) c2 > 100) {
             //printf("DATA char out of range");
-            //send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
             return false;
         }
     }
@@ -264,19 +229,16 @@
     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;
     }
     free(checksum);
@@ -294,57 +256,14 @@
 * @date 2/4/2015
 */
 void dataComm::process_write(short int* msg, int len)
-//void dataComm::process_write(char *msg, int len)
 {
   
   if (msg[0] == 0) {
        return;
     }
-   //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone. 
-   char *message = (char*) msg;
-   /*
-   for (int i = 0; i < len*2; i += 2) {
-       message[i] = msg[i/2] & 0xff;
-       message[i+1] = (msg[i/2] >> 8) & 0xff;
-    }
-   for (int i = 0; i < len*2; i+=1) {
-        //if (message[i] != 0) {
-        pc.printf("%x, ",message[i]);
-        //}
-    }
-    pc.printf("\r\n");*/
-
-/*
-  char *message = convert_to_char_array(msg, len);
- if (message[0] == 0xff) {
+   //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone.
+    char *message = (char*) msg;
 
-    char message[len/2];
-    for (int i = 4; i < len; i += 4) {
-        message[(i-4)/2] = msg[i];
-        message[(i-4)/2+1] = msg[i+1];
-    }*/
-/*
-    for (int j=0;j<len;j+=1){
-        //if (message[j] != 0) {
-        printf("%d: %x\r\n", j, message[j]);
-        //}
-    }
-    //}
-    mbedLED1 = 1;
-/*
-    if (message[2] == END) {
-        _failures += 1;
-        if (_failures < 5) {
-           send(_lastCmd);
-        } else {
-            _failures = 0;
-        }
-        printf("Exited\r\n");
-
-        return;
-    }
-   
-*/
 
     if (!msg_check(message, len*2)) {
         return;
@@ -361,9 +280,9 @@
             return;
         }
         int index = msgc & 0x7f;
-        
+
         _paramMap[_indexMap[index]] = message[i+1];
         generic_set(_indexMap[index], message[i+1]);
-       
+
     }
 }