Revision 14:b2dd17623a3c, committed 2015-06-24

Comitter:

perr1940

Date:

Wed Jun 24 19:15:03 2015 +0000

Parent:

13:624f822a9b76

Commit message:

Added Stance start and end support.

Changed in this revision

diff -r 624f822a9b76 -r b2dd17623a3c dataComm.cpp
--- a/dataComm.cpp	Wed Jun 24 01:08:06 2015 +0000
+++ b/dataComm.cpp	Wed Jun 24 19:15:03 2015 +0000
@@ -18,11 +18,12 @@
 
 
     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", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd",
-        "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
-        "RHipRefAngle", "Charge"};
+                           "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
+                           "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
+                           "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
     for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
         _indexMap[j] = temp2[j];
@@ -69,8 +70,10 @@
         return (short) ((mm_gait_params.time_steps-MIN_STEPTIME)/(MAX_STEPTIME-MIN_STEPTIME)*100);
     } else if (var.compare("PhaseShift") == 0) {
         return (short) ((mm_gait_params.peak_time-MIN_PHASESHIFT)/(MAX_PHASESHIFT-MIN_PHASESHIFT)*100);
+    } else if (var.compare("StanceEnd") == 0) {
+        return (short) ((mm_gait_params.stance_end-MIN_STANCEEND)/(MAX_STANCEEND-MIN_STANCEEND)*100);
     } else if (var.compare("StanceStart") == 0) {
-        return (short) ((mm_gait_params.stance_start-MIN_WALK)/(MAX_WALK-MIN_WALK)*100);
+        return (short) ((mm_gait_params.stance_start-MIN_STANCESTART)/(MAX_STANCESTART-MIN_STANCESTART)*100);
     } else if (var.compare("StepLength") == 0) {
         return (short) ((mm_gait_params.stance_end-MIN_STEPLEN)/(MAX_STEPLEN-MIN_STEPLEN)*100);
     } else if (var.compare("HipFlex") == 0) {
@@ -90,14 +93,14 @@
 void dataComm::generic_set(std::string var, short newval)
 {
     mbedLED3 = 1;
-    //newval is a short from 0-100, and needs to be converted to a raw value. 
+    //newval is a short from 0-100, and needs to be converted to a raw value.
     //We do this by making a 100-point scale between MIN and MAX param values
     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));
+        // 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());
@@ -116,13 +119,15 @@
         //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());
+        // pc.printf("%d\r\n", (short)fsm.get_backbias());
     } else if (var.compare("StepTime")==0) {
         mm_gait_params.time_steps = MIN_STEPTIME+(float)newval/100*(MAX_STEPTIME-MIN_STEPTIME);
     } else if (var.compare("PhaseShift") == 0) {
         mm_gait_params.peak_time = MIN_PHASESHIFT+(float)newval/100*(MAX_PHASESHIFT-MIN_PHASESHIFT);
     } else if (var.compare("StanceStart") == 0) {
-        mm_gait_params.stance_start = MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK);
+        mm_gait_params.stance_start = MIN_STANCESTART+(float)newval/100*(MAX_STANCESTART-MIN_STANCESTART);
+    } else if (var.compare("StanceEnd") == 0) {
+        mm_gait_params.stance_end = MIN_STANCEEND+(float)newval/100*(MAX_STANCEEND-MIN_STANCEEND);
     } else if (var.compare("StepLength") == 0) {
         mm_gait_params.stance_end = MIN_STEPLEN+(float)newval/100*(MAX_STEPLEN-MIN_STEPLEN);
     } else if (var.compare("HipFlex") == 0) {
@@ -276,11 +281,11 @@
 */
 void dataComm::process_write(short int* msg, int len)
 {
-  
-  if (msg[0] == 0) {
-       return;
+
+    if (msg[0] == 0) {
+        return;
     }
-   //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone.
+    //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone.
     char *message = (char*) msg;
 
 

diff -r 624f822a9b76 -r b2dd17623a3c dataComm.h
--- a/dataComm.h	Wed Jun 24 01:08:06 2015 +0000
+++ b/dataComm.h	Wed Jun 24 19:15:03 2015 +0000
@@ -114,6 +114,10 @@
     static const float MAX_STEPTIME = 1300;
     static const float MIN_STANDUPTIME = 0.5;
     static const float MAX_STANDUPTIME = 1.5;
+    static const float MAX_STANCESTART=20.0;
+    static const float MIN_STANCESTART=0.0;
+    static const float MAX_STANCEEND=-5.0;
+    static const float MIN_STANCEEND=-30.0;
     
     static const float MIN_STEPLEN = -32;
     static const float MAX_STEPLEN = -12;