changed to be compatible with struct
Fork of dataComm by
Diff: dataComm.cpp
- Revision:
- 2:53547eb587fb
- Parent:
- 1:ad39c297a768
- Child:
- 3:2091104c9b61
--- a/dataComm.cpp Fri Mar 20 23:08:01 2015 +0000 +++ b/dataComm.cpp Mon Apr 13 17:43:34 2015 +0000 @@ -5,21 +5,11 @@ #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++) { @@ -53,37 +43,50 @@ } +/** +* 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) { - // 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; - //} + 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){ - 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) { @@ -96,7 +99,11 @@ 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)); @@ -164,154 +171,11 @@ } */ -/** -* 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]; @@ -332,12 +196,14 @@ */ 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; } @@ -369,6 +235,7 @@ } 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)) { @@ -379,11 +246,13 @@ 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)); @@ -414,77 +283,8 @@ 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 @@ -495,10 +295,11 @@ 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) { @@ -545,19 +346,10 @@ */ 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"); - + //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) { @@ -570,102 +362,7 @@ 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