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