changed to be compatible with struct
Fork of dataComm by
dataComm.cpp
- Committer:
- mzling
- Date:
- 2015-04-24
- Revision:
- 3:2091104c9b61
- Parent:
- 2:53547eb587fb
- Child:
- 4:86d9e33652b6
File content as of revision 3:2091104c9b61:
#include "mbed.h" //#include "MODSERIAL.h" #include "dataComm.h" #include "initExoVars.h" #include "gaitGenerator.h" #include <string> #include <map> 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}; 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", "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"]); } /** * 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) { 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); } 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; //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()); } } /** * 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; } /** * 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 * @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"); //send_error(START_ERR); 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 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"); // 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; 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"); //send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR)); 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"); //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; } } 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); // 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); 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) //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) { 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; } 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]); } }