changed to be compatible with struct
Fork of dataComm by
Diff: dataComm.cpp
- Revision:
- 0:f0dc2775ec68
- Child:
- 1:ad39c297a768
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dataComm.cpp Fri Mar 20 20:25:34 2015 +0000 @@ -0,0 +1,664 @@ +#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; + }else if(var.compare( "BentForwardAngle")==0){ + return (short)bentAngle; + /* } else if (var.compare("StandupAsst")==0){ + return (short)fsm.get_standup_asst(); + } else if (var.compare("SitdownAsst")==0) { + return (short)fsm.get_sitdown_asst(); + } else if (var.compare("SitdownTime")==0){ + return (short)tSittingDown; + } else if (var.compare( "StandingAngle")==0){ + return (short)stand_adjust; + } else if (var.compare("WalkAngle")==0){ + return (short)fsm.get_backbias();*/ + } + // default: + return 0; + //} +} + +void dataComm::generic_set(std::string var, short newval) +{ + mbedLED3 = 1; + if (var.compare( "SittingAngle")==0){ + // sittingAngle = (float)newval; + } else if (var.compare( "BentForwardAngle")==0){ + motorLED = 1; + //bentAngle = (float)newval; + } else if (var.compare("StandupAsst")==0) { + // fsm.set_standup_asst((float)newval); + } else if (var.compare("SitdownAsst")==0) { + // fsm.set_sitdown_asst((float)newval); + } else if (var.compare("SitdownTime")==0) { + // tSittingDown = (float)newval; + } else if (var.compare("StandingAngle")==0) { + // stand_adjust = float(newval); + } else if (var.compare("WalkAngle")==0) { + // fsm.set_backbias((float)newval); + } + +} +/** +* 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); + + } +} +*/ \ No newline at end of file