fs and ftg working. bent forward not working
Fork of dataComm by
dataComm.cpp
- Committer:
- mzling
- Date:
- 2015-03-20
- Revision:
- 1:ad39c297a768
- Parent:
- 0:f0dc2775ec68
- Child:
- 2:53547eb587fb
File content as of revision 1:ad39c297a768:
#include "mbed.h" //#include "MODSERIAL.h" #include "dataComm.h" #include "initExoVars.h" #include "gaitGenerator.h" #include <string> #include <map> //Variable indices for set_values: set_values should take an array of length NUMVARS + NUMREADONLYPARAMS = 34. //Map of variables to array indices is as follows: //0 = Torso Angle, 1 = Left Knee Angle, 2 = Right Knee Angle, 3 = Left Hip angle, 4 = Right Hip angle, 5 = Left Hip Torque, 6 = Right Hip Torque //8 = Exo State/Left Knee State/Right Knee State 9 = Torso Ref. Angle, 10 = Left Hip Ref. Angle, 11 = Right Hip Ref. Angle //12 = KP Stance, 13 = KP Swing, 14 = KP Standing, 15 = KP Sitting, 16 = KP Standing up, 17 = KP Sitting down //18 = KD Stance, 19 = KD Swing, 20 = KD Standing, 21 = KD Sitting, 22 = KD Standing up, 23 = KD sitting down //24 = Standing angle, 25 = Sitting angle, 26 = Bent Forward Angle, 27 = Forward Angle, 28 = Rear Angle, 29 = IMU Angle //30 = Knee Full Retract, 31 = Knee Full Extend, 32 = Lock Time, 33 = Rate dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(27), _numReadOnlyParams(12), _escapesNeeded(8) { /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */ //_rn42.baud(9600); //pc.printf("Started BTComm init \r\n"); //_rn42.baud(115200); 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", "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"]); } short dataComm::generic_get(std::string var) { // switch (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); } // default: return 0; //} } void dataComm::generic_set(std::string var, short newval) { mbedLED3 = 1; 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){ motorLED = 1; 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) { stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND); //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); } */ /** * Sends the specified char array through the _rn42 Bluetooth connection. * @param cmd The char array to be sent * @author Michael Ling * @date 2/4/2015 void dataComm::send(char *cmd) { for (int i = 0; i < 50; i++) { _rn42.putc(cmd[i]); } } */ /** * Sets the parameter map, based on the input map NEWVALUES * @param newValues A map of strings to shorts to be copied to _paramMap * @author Michael Ling * @date 2/4/2015 */ void dataComm::set_values(std::map<string, short> newValues) { for (std::map<string, short>::iterator it = newValues.begin(); it != newValues.end(); ++it) { _paramMap[it->first] = it->second; } } /** * Sends the paramList with START/END, parity bits, and a checksum * @param paramList List of parameters to be sent over Bluetooth, represented as a char array * @author Michael Ling * @date 2/4/2015 void dataComm::send_values(char* paramList) { char msg[2*_numVars+6]; int len=2; //printf("Sending values \r\n"); msg[0] = START; msg[1] = 0; for (int i=0; i < _numVars; i++) { if (i == 21) { //printf("On final loop \r\n"); } if (paramList[i] != 0xff) { short s = (short)((i << 8) | paramList[i]); //printf("In send_values, calculating parity of %x\r\n", s); if (parity(s)) { //printf("%x requires TRUE parity bit\r\n", s); msg[len] = (char)(i | 0x40); len += 1; } else { //printf("%x requires FALSE parity bit\r\n", s); msg[len] = (char)i; len += 1; } msg[len] = paramList[i]; len += 1; } } msg[len] = END; len += 1; char* checksum = get_checksum(msg, len); msg[len] = checksum[0]; msg[len+1] = checksum[1]; msg[len+2] = checksum[2]; len += 3; for (int j = 0; j < len; j++) { //printf("Sending char %x \r\n", msg[j]); _rn42.putc(msg[j]); } memcpy(_lastCmd, msg, 50); free(checksum); return ; } */ /** * Sends readOnly Parameters, with START/END and checksum * @author Michael Ling * @date 2/4/2015 void dataComm::send_read_only_values() { //printf("SENDING RO VALUES!\r\n"); int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7; // printf("msglen is %d\r\n", msgLen); char message[msgLen]; // printf("message of len 39 created\r\n"); message[0] = START; //printf("%x\r\n", message[0]); message[1] = 0; //printf("%x\r\n", message[1]); message[2] = READONLY_IND; //printf("3 bytes of message set\r\n"); int msgInd = 3; int escapes = 0; //printf("%d readonly parameters", _numReadOnlyParams); for (int i = 0; i < _numReadOnlyParams; i++) { if (i == _escapeNeeded[escapes]) { pc.printf("Escape char. needed at index %d \r\n", i); //char conflict = (char)(_readOnlyParams[i] & 0xff); char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff); //printf("%x possibly has a conflict in %x \r\n", _readOnlyParams[i], conflict); escapes += 1; //message[msgInd+1] = (char) (_readOnlyParams[i] >> 8); message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8); printf("Set msgInd+1 to %x \r\n", message[msgInd+1]); if (conflict == (char) 0xfe) { message[msgInd] = 1; message[msgInd+2] = 0xfc; } else if (conflict == (char) 0xff) { message[msgInd] = 1; message[msgInd+2] = 0xfd; } else { message[msgInd] = 0; message[msgInd+2] = conflict; } msgInd += 3; } else { // message[msgInd] = (char) (_readOnlyParams[i] >> 8); // message[msgInd+1] = (char) (_readOnlyParams[i] & 0xff); message[msgInd] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8); message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] & 0xff); msgInd += 2; } } message[msgLen-4] = END; char* checksum = get_checksum(message, msgLen-3); message[msgLen-3] = checksum[0]; message[msgLen-2] = checksum[1]; message[msgLen-1] = checksum[2]; //printf("Sending the following readONly values: \r\n"); for (int j=0; j < msgLen; j++) { pc.printf("%x \r\n", message[j]); _rn42.putc(message[j]); } memcpy(_lastCmd, message, 50); free(checksum); //printf("Finished sending readOnly values \r\n"); } */ 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) { if (message[0] != START) { //printf("Improper START or END \r\n"); //send_error(START_ERR); return false; } //printf("got a start\r\n"); 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); } 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]; 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; } 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 a received readOnly message * @param message The readonly message received * @param len Length of the message * @author Michael Ling * @date 2/4/2015 void dataComm::process_read_only(char* message, int len) { //printf("Message is a ReadOnly \r\n"); if (!msg_check(message, len)) { pc.printf("msg_check failed on readonly! \r\n"); return; } //PASS TO CTRLBED _failures = 0; //pc.printf("Sending readOnly values \r\n"); send_read_only_values(); } /** * Checks received READ message, and places requested data into an array * @param message The received READ request as a char array * @param len Length of the READ message * @author Michael Ling * @date 2/4/2015 void dataComm::process_read(char* message, int len) { //If the received message is an error message, resend the last command if (message[2] == END) { _failures += 1; //printf("_failures: %d\r\n", _failures); if (_failures < 5) { send(_lastCmd); } else { _failures = 0; } return; } if (!msg_check(message, len)) { //printf("msg_check failed on read! \r\n"); return; } //PASS MSG TO CTRLBED _failures = 0; //printf("Message is a read \r\n"); char paramList[_numVars]; memset(paramList, 0xff, _numVars); for (int i=2; i < len-5; i++) { char msg = message[i] & 0xbf; if ((msg & 0x80) != 0) { // printf("Got a non-read char %x...exiting \r\n", msg); return; } int index = msg & 0xff; //printf("Value at index %d requested \r\n", index); paramList[index] = _paramMap[_indexMap[index]]; } if (message[len-5] == READONLY_IND) { pc.printf("Need to send RO vals\r\n"); send_read_only_values(); } printf("About to send PARAMLIST\r\n"); send_values(paramList); } */ /** * 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; } 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)) { //printf("msg_check failed on write! \r\n"); //printf("X\r\n"); return; } // pc.printf("MSG check passed!\r\n"); mbedLED2 = 1; //PASS msg. to ControlBed //char paramList[_numVars]; //memset(paramList, 0xff, _numVars); //printf("Message is a write \r\n"); 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]; // paramList[index] = message[i+1]; generic_set(_indexMap[index], message[i+1]); //printf("Wrote %x to index %d of localValues \r\n", localValues[index], index); } // send_values(paramList); // pc.printf("Exited\r\n"); } /** * Checks if received message is a read, write or readonly * @param message The received message * @param len Length of the message * @author Michael Ling * @date 2/4/2015 void dataComm::process (char* message, int len) { char c = message[2]; for (int i =0; i < len; i++) { //printf("Message character: %x \r\n", message[i]); } if (c == READONLY_IND) { process_read_only(message, len); return; } if ((c & 0x80) == 0) { //printf("Is a read\r\n"); process_read(message, len); return; } else { process_write(message, len); return; } } */ //Warning: do not put print statements in the function attachment(); it will interfere with receiving messages /** * Scans for data received through Bluetooth, and passes it on if it detects a message-like chunk. Should be run via an interuupt. * @author Michael Ling * @date 2/4/2015 void dataComm::attachment() { boardLed1 = !boardLed1; // pc.printf("Entered attachment\r\n"); if (_rn42.readable()) { _data=_rn42.getc(); // if (_data != NULL) { char b = _data & 0xff; //printf("Got char: %x \r\n", b); if (b != NULL or _inMsg) { // printf("Got char non null: %x \r\n", b); } //This marks the START of a message if (_inMsg == false and b == START) { // printf("Msg START received \r\n"); _inMsg = true; _counter = 3; _curMsg[_len] = b; _len += 1; } else if (_inMsg == true and b == START) { // printf("Second start received, terminating\r\n"); _inMsg = false; _counter = 0; memset(_curMsg, 0, 50); _rn42.rxBufferFlush(); process(_msg, _len); _len = 0; } else if (_inMsg || _counter > 0 ) { // printf("_inMsg or _counter > 0 \r\n"); _curMsg[_len] = b; _len += 1; if (!_inMsg) { _counter -= 1; } //Marks end of message, and starts processing if (_counter <= 0) { // printf("End of message \r\n"); memset(_msg, 0, 50); memcpy(_msg, _curMsg, 50); memset(_curMsg, 0, 50); _rn42.rxBufferFlush(); process(_msg, _len); _len = 0; } } if (b == END) { _inMsg = false; // _rn42.putc(msg); } //_rn42.putc(_data); } } */