changed to be compatible with struct
Fork of dataComm by
Diff: dataComm.cpp
- Revision:
- 4:86d9e33652b6
- Parent:
- 3:2091104c9b61
- Child:
- 5:c95a0006ba68
diff -r 2091104c9b61 -r 86d9e33652b6 dataComm.cpp --- 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]); - + } }