fs and ftg working. bent forward not working
Fork of dataComm by
dataComm.cpp
- Committer:
- perr1940
- Date:
- 2015-06-24
- Revision:
- 14:b2dd17623a3c
- Parent:
- 13:624f822a9b76
File content as of revision 14:b2dd17623a3c:
#include "mbed.h" #include "dataComm.h" #include "initExoVars.h" #include "gaitGenerator.h" #include <string> #include <map> //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}; 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", "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]; } 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}; for (int k = 0; k < _numReadOnlyParams; k += 1) { _paramMap[_indexMap[_numVars + k]] = temp4[k]; } //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]); } /** * Returns the stored value of the specified variable * @param var The variable to be gotten (as a string). * @author Michael Ling * @date 3/31/2015 */ 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) { return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100); } 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) { return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100); } else if (var.compare( "StandingAngle")==0) { return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100); } 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) ((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_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) { return (short) ((mm_gait_params.max_angle-MIN_HIPFLEX)/(MAX_HIPFLEX-MIN_HIPFLEX)*100); } return 0; } /** * Sets a new value for the given variable * @param var The variable to be set, as a string * @param newval New value for the variable * @author Michael Ling * @date 3/31/2015 */ 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. //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)); } 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) { 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_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) { mm_gait_params.max_angle = MIN_HIPFLEX+(float)newval/100*(MAX_HIPFLEX-MIN_HIPFLEX); } //MORE else blocks for gait params } /** * Calculates parity--0 if c even, 1 if odd * @param c The short that we want to calculate parity of * @author Michael Ling * @date 2/4/2015 */ bool dataComm::parity(short c) { bool p = false; while (c != 0) { p = !p; c = (short) (c & (c-1)); } return p; } /** * Calculates checksum of char array, sum of all chars in array * @param b The char array to be summed up * @param len Length of the array b * @author Michael Ling * @date 2/4/2015 */ char* dataComm::get_checksum(char* b, int len) { char* checksum = (char*)malloc(3*sizeof(char)); short sum = 0; for (int i = 0; i < len; i += 1) { sum += ((short) b[i] & 0xff); } checksum[1] = (char) ((sum >> 8) & 0xff); checksum[2] = (char) (sum & 0xff); char check = (char) (sum & 0xff); //Sets escape character if the checksum contains START or END if (check == START || check == END) { checksum[0] = 1; checksum[2] = (char) (checksum[2] & 0x1); } else { checksum[0] = 0; } return checksum; } /** * 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 * @param message The received message to check * @param len Length of the message * @author Michael Ling * @date 2/4/2015 */ bool dataComm::msg_check(char* message, int len) { //Checks the message starts with 0xff if (message[0] != START) { //printf("Improper START or END \r\n"); return false; } //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; } if (message[len-4] != END) { //send_error(END_ERR); return false; } bool write = message[2] & 0x80; for (int i=2; i < len-5; i+=2) { if (i == 2 && message[i] == READONLY_IND) { break; } if (i == (len-5) && message[i] == READONLY_IND) { break; } if (((message[i] & 0x80) !=0) && !write) { //printf("Does not match READ format \r\n"); return false; } if (((message[i] & 0x80) != 0x80) && write) { //printf("char %x Does not match WRITE format \r\n", message[i]); return false; } short s; if (write) { s = (short) ((message[i] << 8) | message[i+1]); } else { s = (short) (message[i] << 8); } //Checks the parity bit of the var/data pair. bool parity1 = (s & 0x4000) != 0; if (parity1 != parity(s & 0x4000)) { //printf("Parity error in VAR char \r\n"); return false; } char c = message[i] & 0x3f; char c2 = message[i+1]; //Makes sure the var is a valid one if ((int) c < 0 || (int) c > _numVars) { //printf("VAR char out of range \r\n"); 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"); 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); 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); 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); return false; } free(checksum); return true; } /** * Checks received WRITE message and writes to paramMap/SDCard * @param message The received WRITE message * @param len Length of the WRITE message * @author Michael Ling * @date 2/4/2015 */ void dataComm::process_write(short int* 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; if (!msg_check(message, len*2)) { return; } mbedLED2 = 1; //If the msgCheck passes, we change the local paramMap and set variables to their new values for (int i=2; i < len*2-5; i+=2) { //printf("Loop %d\r\n", i); if (message[i] == END) { return; } char msgc = message[i] & 0xbf; if ((msgc & 0x80) != 0x80) { return; } int index = msgc & 0x7f; _paramMap[_indexMap[index]] = message[i+1]; generic_set(_indexMap[index], message[i+1]); } }