fs and ftg working. bent forward not working
Fork of dataComm by
Diff: dataComm.cpp
- Revision:
- 6:502959ea39e4
- Parent:
- 5:c95a0006ba68
- Child:
- 7:26d706d285c1
diff -r c95a0006ba68 -r 502959ea39e4 dataComm.cpp --- a/dataComm.cpp Wed May 06 17:13:38 2015 +0000 +++ b/dataComm.cpp Mon May 11 21:46:44 2015 +0000 @@ -6,7 +6,8 @@ #include <string> #include <map> -dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8) +//Much code is shared with BluetoothComm class, on the control bed +dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8) { mbedLED1 = 1; int temp1[] = {0,1,2,3,4,8,9,10,-1}; @@ -19,7 +20,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 @@ -49,6 +50,7 @@ */ short dataComm::generic_get(std::string var) { + //Translate the raw value of the parameter to a short between 0-100 if (var.compare("SittingAngle") == 0) { return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100); } else if(var.compare( "BentForwardAngle")==0) { @@ -64,7 +66,15 @@ } 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 (short) ((MMgait.time_steps-MIN_STEPTIME)/(MAX_STEPTIME-MIN_STEPTIME)*100); + } else if (var.compare("PhaseShift") == 0) { + return (short) ((MMgait.peak_time-MIN_PHASESHIFT)/(MAX_PHASESHIFT-MIN_PHASESHIFT)*100); + } else if (var.compare("WalkAngle") == 0) { + return (short) ((MMgait.walking_angle-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); + } else if (var.compare("StepLength") == 0) { + return (short) ((MMgait.end_angle-MIN_STEPLEN)/(MAX_STEPLEN-MIN_STEPLEN)*100); + } else if (var.compare("HipFlex") == 0) { + return (short) ((MMgait.max_angle-MIN_HIPFLEX)/(MAX_HIPFLEX-MIN_HIPFLEX)*100); } return 0; @@ -80,8 +90,8 @@ 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 + //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)); @@ -108,7 +118,15 @@ 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; + MMgait.time_steps = (float)newval/100*(MAX_STEPTIME-MIN_STEPTIME); + } else if (var.compare("PhaseShift") == 0) { + MMgait.peak_time = (float)newval/100*(MAX_PHASESHIFT-MIN_PHASESHIFT); + } else if (var.compare("WalkAngle") == 0) { + MMgait.walking_angle = (float)newval/100*(MAX_WALK-MIN_WALK); + } else if (var.compare("StepLength") == 0) { + MMgait.end_angle = (float)newval/100*(MAX_STEPLEN-MIN_STEPLEN); + } else if (var.compare("HipFlex") == 0) { + MMgait.max_angle = (float)newval/100*(MAX_HIPFLEX-MIN_HIPFLEX); } //MORE else blocks for gait params @@ -175,6 +193,7 @@ } //printf("got a start\r\n"); //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END + //Otherwise, the msg_check procedure is the same while (message[len-4] != END && len >= 6) { len -= 1; }