Hayley Cashdollar
fs and ftg working. bent forward not working
Fork of
dataComm
by 
Bradley Perry
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