Hayley Cashdollar / dataComm

fs and ftg working. bent forward not working

Fork of dataComm by Bradley Perry

dataComm.cpp@3:2091104c9b61, 2015-04-24 (annotated)

Committer:
mzling
Date:
Fri Apr 24 19:34:49 2015 +0000
Revision:
3:2091104c9b61
Parent:
2:53547eb587fb
Child:
4:86d9e33652b6
Added gait params

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mzling 0:f0dc2775ec68 1 #include "mbed.h"
mzling 0:f0dc2775ec68 2 //#include "MODSERIAL.h"
mzling 0:f0dc2775ec68 3 #include "dataComm.h"
mzling 0:f0dc2775ec68 4 #include "initExoVars.h"
mzling 0:f0dc2775ec68 5 #include "gaitGenerator.h"
mzling 0:f0dc2775ec68 6 #include <string>
mzling 0:f0dc2775ec68 7 #include <map>
mzling 2:53547eb587fb 8
mzling 3:2091104c9b61 9 dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8)
mzling 0:f0dc2775ec68 10 {
mzling 0:f0dc2775ec68 11
mzling 2:53547eb587fb 12
mzling 0:f0dc2775ec68 13 int temp1[] = {0,1,2,3,4,8,9,10, -1};
mzling 0:f0dc2775ec68 14
mzling 0:f0dc2775ec68 15 for (int i = 0; i < _escapesNeeded+1; i++) {
mzling 0:f0dc2775ec68 16 _escapeNeeded[i] = temp1[i];
mzling 0:f0dc2775ec68 17 }
mzling 0:f0dc2775ec68 18
mzling 0:f0dc2775ec68 19
mzling 0:f0dc2775ec68 20 std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
mzling 0:f0dc2775ec68 21 "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
mzling 3:2091104c9b61 22 "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
mzling 3:2091104c9b61 23 "StepLength", "StepTime", "PeakFlexion", "PhaseShift", "WalkSpeed",
mzling 0:f0dc2775ec68 24 "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
mzling 0:f0dc2775ec68 25 "RHipRefAngle", "Charge"};
mzling 0:f0dc2775ec68 26 //Populate the map of indices to param names
mzling 0:f0dc2775ec68 27 for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
mzling 0:f0dc2775ec68 28 _indexMap[j] = temp2[j];
mzling 0:f0dc2775ec68 29 }
mzling 0:f0dc2775ec68 30
mzling 0:f0dc2775ec68 31 for (int j = 0; j < _numVars; j += 1) {
mzling 0:f0dc2775ec68 32 _paramMap[_indexMap[j]] = generic_get(_indexMap[j]);
mzling 0:f0dc2775ec68 33 }
mzling 0:f0dc2775ec68 34
mzling 0:f0dc2775ec68 35 // pc.printf("Initialized PARAM \r\n");
mzling 0:f0dc2775ec68 36 int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
mzling 0:f0dc2775ec68 37 //readData.write(_numReadOnlyParams, temp4);
mzling 0:f0dc2775ec68 38 for (int k = 0; k < _numReadOnlyParams; k += 1) {
mzling 0:f0dc2775ec68 39 _paramMap[_indexMap[_numVars + k]] = temp4[k];
mzling 0:f0dc2775ec68 40 }
mzling 0:f0dc2775ec68 41 //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]);
mzling 0:f0dc2775ec68 42
mzling 0:f0dc2775ec68 43
mzling 0:f0dc2775ec68 44
mzling 0:f0dc2775ec68 45 }
mzling 0:f0dc2775ec68 46
mzling 2:53547eb587fb 47 /**
mzling 2:53547eb587fb 48 * Returns the stored value of the specified variable
mzling 2:53547eb587fb 49 * @param var The variable to be gotten (as a string).
mzling 2:53547eb587fb 50 * @author Michael Ling
mzling 2:53547eb587fb 51 * @date 3/31/2015
mzling 2:53547eb587fb 52 */
mzling 0:f0dc2775ec68 53 short dataComm::generic_get(std::string var)
mzling 0:f0dc2775ec68 54 {
mzling 2:53547eb587fb 55 if (var.compare("SittingAngle") == 0){
mzling 2:53547eb587fb 56 return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100);
mzling 2:53547eb587fb 57 }else if(var.compare( "BentForwardAngle")==0){
mzling 2:53547eb587fb 58 return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100);
mzling 2:53547eb587fb 59 } else if (var.compare("StandupAsst")==0){
mzling 2:53547eb587fb 60 return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100);
mzling 2:53547eb587fb 61 } else if (var.compare("SitdownAsst")==0) {
mzling 2:53547eb587fb 62 return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100);
mzling 2:53547eb587fb 63 } else if (var.compare("SitdownTime")==0){
mzling 2:53547eb587fb 64 return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100);
mzling 2:53547eb587fb 65 } else if (var.compare( "StandingAngle")==0){
mzling 2:53547eb587fb 66 return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100);
mzling 2:53547eb587fb 67 } else if (var.compare("WalkAngle")==0){
mzling 2:53547eb587fb 68 return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100);
mzling 2:53547eb587fb 69 }
mzling 2:53547eb587fb 70 return 0;
mzling 2:53547eb587fb 71
mzling 0:f0dc2775ec68 72 }
mzling 0:f0dc2775ec68 73
mzling 2:53547eb587fb 74 /**
mzling 2:53547eb587fb 75 * Sets a new value for the given variable
mzling 2:53547eb587fb 76 * @param var The variable to be set, as a string
mzling 2:53547eb587fb 77 * @param newval New value for the variable
mzling 2:53547eb587fb 78 * @author Michael Ling
mzling 2:53547eb587fb 79 * @date 3/31/2015
mzling 2:53547eb587fb 80 */
mzling 0:f0dc2775ec68 81 void dataComm::generic_set(std::string var, short newval)
mzling 0:f0dc2775ec68 82 {
mzling 0:f0dc2775ec68 83 mbedLED3 = 1;
mzling 3:2091104c9b61 84 //pc.printf("%s\r\n", var);
mzling 2:53547eb587fb 85 //newval is a short from 0-100, and needs to be converted a more
mzling 0:f0dc2775ec68 86 if (var.compare( "SittingAngle")==0){
mzling 1:ad39c297a768 87 sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT);
mzling 1:ad39c297a768 88 // pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100));
mzling 0:f0dc2775ec68 89 } else if (var.compare( "BentForwardAngle")==0){
mzling 2:53547eb587fb 90
mzling 1:ad39c297a768 91 bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT);
mzling 1:ad39c297a768 92 // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100));
mzling 0:f0dc2775ec68 93 } else if (var.compare("StandupAsst")==0) {
mzling 1:ad39c297a768 94 fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST));
mzling 1:ad39c297a768 95 // pc.printf("%d\r\n", (short)fsm.get_standup_asst());
mzling 0:f0dc2775ec68 96 } else if (var.compare("SitdownAsst")==0) {
mzling 1:ad39c297a768 97 fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST));
mzling 1:ad39c297a768 98 // pc.printf("%d\r\n", (short)fsm.get_sitdown_asst());
mzling 0:f0dc2775ec68 99 } else if (var.compare("SitdownTime")==0) {
mzling 1:ad39c297a768 100 tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME);
mzling 1:ad39c297a768 101 // pc.printf("%d\r\n", (short)tSittingDown);
mzling 0:f0dc2775ec68 102 } else if (var.compare("StandingAngle")==0) {
mzling 2:53547eb587fb 103 pc.printf("LED\r\n");
mzling 2:53547eb587fb 104 motorLED = 1;
mzling 1:ad39c297a768 105 stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND);
mzling 2:53547eb587fb 106 encoder_L.init(zero_ang_L+stand_adjust);
mzling 2:53547eb587fb 107 encoder_R.init(zero_ang_R+stand_adjust);
mzling 1:ad39c297a768 108 //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100));
mzling 0:f0dc2775ec68 109 } else if (var.compare("WalkAngle")==0) {
mzling 1:ad39c297a768 110 fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK));
mzling 1:ad39c297a768 111 // pc.printf("%d\r\n", (short)fsm.get_backbias());
mzling 0:f0dc2775ec68 112 }
mzling 0:f0dc2775ec68 113
mzling 0:f0dc2775ec68 114 }
mzling 0:f0dc2775ec68 115 /**
mzling 0:f0dc2775ec68 116 * Calculates parity--0 if c even, 1 if odd
mzling 0:f0dc2775ec68 117 * @param c The short that we want to calculate parity of
mzling 0:f0dc2775ec68 118 * @author Michael Ling
mzling 0:f0dc2775ec68 119 * @date 2/4/2015
mzling 0:f0dc2775ec68 120 */
mzling 0:f0dc2775ec68 121 bool dataComm::parity(short c)
mzling 0:f0dc2775ec68 122 {
mzling 0:f0dc2775ec68 123 bool p = false;
mzling 0:f0dc2775ec68 124 while (c != 0) {
mzling 0:f0dc2775ec68 125 p = !p;
mzling 0:f0dc2775ec68 126 c = (short) (c & (c-1));
mzling 0:f0dc2775ec68 127 }
mzling 0:f0dc2775ec68 128 return p;
mzling 0:f0dc2775ec68 129 }
mzling 0:f0dc2775ec68 130
mzling 0:f0dc2775ec68 131 /**
mzling 0:f0dc2775ec68 132 * Calculates checksum of char array, sum of all chars in array
mzling 0:f0dc2775ec68 133 * @param b The char array to be summed up
mzling 0:f0dc2775ec68 134 * @param len Length of the array b
mzling 0:f0dc2775ec68 135 * @author Michael Ling
mzling 0:f0dc2775ec68 136 * @date 2/4/2015
mzling 0:f0dc2775ec68 137 */
mzling 0:f0dc2775ec68 138 char* dataComm::get_checksum(char* b, int len)
mzling 0:f0dc2775ec68 139 {
mzling 0:f0dc2775ec68 140 char* checksum = (char*)malloc(3*sizeof(char));
mzling 0:f0dc2775ec68 141 short sum = 0;
mzling 0:f0dc2775ec68 142 for (int i = 0; i < len; i += 1) {
mzling 0:f0dc2775ec68 143 sum += ((short) b[i] & 0xff);
mzling 0:f0dc2775ec68 144 }
mzling 0:f0dc2775ec68 145 checksum[1] = (char) ((sum >> 8) & 0xff);
mzling 0:f0dc2775ec68 146 checksum[2] = (char) (sum & 0xff);
mzling 0:f0dc2775ec68 147 char check = (char) (sum & 0xff);
mzling 0:f0dc2775ec68 148 //Sets escape character if the checksum contains START or END
mzling 0:f0dc2775ec68 149 if (check == START || check == END) {
mzling 0:f0dc2775ec68 150 checksum[0] = 1;
mzling 0:f0dc2775ec68 151 checksum[2] = (char) (checksum[2] & 0x1);
mzling 0:f0dc2775ec68 152 } else {
mzling 0:f0dc2775ec68 153 checksum[0] = 0;
mzling 0:f0dc2775ec68 154 }
mzling 0:f0dc2775ec68 155 return checksum;
mzling 0:f0dc2775ec68 156 }
mzling 0:f0dc2775ec68 157
mzling 0:f0dc2775ec68 158 /**
mzling 0:f0dc2775ec68 159 * Sends error message.
mzling 0:f0dc2775ec68 160 * @param errCode character representation of the failure-causing error
mzling 0:f0dc2775ec68 161 * @author Michael Ling
mzling 0:f0dc2775ec68 162 * @data 2/4/2015
mzling 0:f0dc2775ec68 163
mzling 0:f0dc2775ec68 164 void dataComm::send_error(char errCode)
mzling 0:f0dc2775ec68 165 {
mzling 0:f0dc2775ec68 166 _rn42.putc(START);
mzling 0:f0dc2775ec68 167 _rn42.putc(errCode);
mzling 0:f0dc2775ec68 168 _rn42.putc(END);
mzling 0:f0dc2775ec68 169 _rn42.putc(0);
mzling 0:f0dc2775ec68 170 _rn42.putc(0);
mzling 0:f0dc2775ec68 171 _rn42.putc(0);
mzling 0:f0dc2775ec68 172 }
mzling 0:f0dc2775ec68 173 */
mzling 0:f0dc2775ec68 174
mzling 0:f0dc2775ec68 175
mzling 0:f0dc2775ec68 176
mzling 0:f0dc2775ec68 177
mzling 0:f0dc2775ec68 178
mzling 0:f0dc2775ec68 179
mzling 0:f0dc2775ec68 180
mzling 0:f0dc2775ec68 181 char* dataComm::convert_to_char_array(short int* message, int len) {
mzling 0:f0dc2775ec68 182 char *retval = new char[len*2];
mzling 0:f0dc2775ec68 183 for (int i = 2; i < len; i+=2) {
mzling 0:f0dc2775ec68 184 short int val = message[i];
mzling 0:f0dc2775ec68 185 retval[(i-1)*2] = (val >> 8) & 0xff;
mzling 0:f0dc2775ec68 186 retval[(i-1)*2+1] = val & 0xff;
mzling 0:f0dc2775ec68 187 }
mzling 0:f0dc2775ec68 188 return retval;
mzling 0:f0dc2775ec68 189 }
mzling 0:f0dc2775ec68 190
mzling 0:f0dc2775ec68 191 /**
mzling 0:f0dc2775ec68 192 * 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
mzling 0:f0dc2775ec68 193 * @param message The received message to check
mzling 0:f0dc2775ec68 194 * @param len Length of the message
mzling 0:f0dc2775ec68 195 * @author Michael Ling
mzling 0:f0dc2775ec68 196 * @date 2/4/2015
mzling 0:f0dc2775ec68 197 */
mzling 0:f0dc2775ec68 198 bool dataComm::msg_check(char* message, int len)
mzling 0:f0dc2775ec68 199 {
mzling 2:53547eb587fb 200 //Checks the message starts with 0xff
mzling 0:f0dc2775ec68 201 if (message[0] != START) {
mzling 0:f0dc2775ec68 202 //printf("Improper START or END \r\n");
mzling 0:f0dc2775ec68 203 //send_error(START_ERR);
mzling 0:f0dc2775ec68 204 return false;
mzling 0:f0dc2775ec68 205 }
mzling 0:f0dc2775ec68 206 //printf("got a start\r\n");
mzling 2:53547eb587fb 207 //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END
mzling 0:f0dc2775ec68 208 while (message[len-4] != END && len >= 6) {
mzling 0:f0dc2775ec68 209 len -= 1;
mzling 0:f0dc2775ec68 210 }
mzling 0:f0dc2775ec68 211 if (message[len-4] != END) {
mzling 0:f0dc2775ec68 212 //send_error(END_ERR);
mzling 0:f0dc2775ec68 213 return false;
mzling 0:f0dc2775ec68 214 }
mzling 0:f0dc2775ec68 215 bool write = message[2] & 0x80;
mzling 0:f0dc2775ec68 216 for (int i=2; i < len-5; i+=2) {
mzling 0:f0dc2775ec68 217 if (i == 2 && message[i] == READONLY_IND) {
mzling 0:f0dc2775ec68 218 break;
mzling 0:f0dc2775ec68 219 }
mzling 0:f0dc2775ec68 220 if (i == (len-5) && message[i] == READONLY_IND) {
mzling 0:f0dc2775ec68 221 break;
mzling 0:f0dc2775ec68 222 }
mzling 0:f0dc2775ec68 223 if (((message[i] & 0x80) !=0) && !write) {
mzling 0:f0dc2775ec68 224 //printf("Does not match READ format \r\n");
mzling 0:f0dc2775ec68 225 // send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 0:f0dc2775ec68 226 return false;
mzling 0:f0dc2775ec68 227 }
mzling 0:f0dc2775ec68 228 if (((message[i] & 0x80) != 0x80) && write) {
mzling 0:f0dc2775ec68 229 //printf("char %x Does not match WRITE format \r\n", message[i]);
mzling 0:f0dc2775ec68 230 //send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 0:f0dc2775ec68 231 return false;
mzling 0:f0dc2775ec68 232 }
mzling 0:f0dc2775ec68 233 short s;
mzling 0:f0dc2775ec68 234 if (write) {
mzling 0:f0dc2775ec68 235 s = (short) ((message[i] << 8) | message[i+1]);
mzling 0:f0dc2775ec68 236 } else {
mzling 0:f0dc2775ec68 237 s = (short) (message[i] << 8);
mzling 0:f0dc2775ec68 238 }
mzling 2:53547eb587fb 239 //Checks the parity bit of the var/data pair.
mzling 0:f0dc2775ec68 240 bool parity1 = (s & 0x4000) != 0;
mzling 0:f0dc2775ec68 241
mzling 0:f0dc2775ec68 242 if (parity1 != parity(s & 0x4000)) {
mzling 0:f0dc2775ec68 243 //printf("Parity error in VAR char \r\n");
mzling 0:f0dc2775ec68 244 //send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
mzling 0:f0dc2775ec68 245 return false;
mzling 0:f0dc2775ec68 246 }
mzling 0:f0dc2775ec68 247
mzling 0:f0dc2775ec68 248 char c = message[i] & 0x3f;
mzling 0:f0dc2775ec68 249 char c2 = message[i+1];
mzling 2:53547eb587fb 250 //Makes sure the var is a valid one
mzling 0:f0dc2775ec68 251 if ((int) c < 0 || (int) c > _numVars) {
mzling 0:f0dc2775ec68 252 //printf("VAR char out of range \r\n");
mzling 0:f0dc2775ec68 253 //send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
mzling 0:f0dc2775ec68 254 return false;
mzling 0:f0dc2775ec68 255 }
mzling 2:53547eb587fb 256 //Makes sure the new value is in the range 0-100
mzling 0:f0dc2775ec68 257 if ((int) c2 < 0 || (int) c2 > 100) {
mzling 0:f0dc2775ec68 258 //printf("DATA char out of range");
mzling 0:f0dc2775ec68 259 //send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
mzling 0:f0dc2775ec68 260 return false;
mzling 0:f0dc2775ec68 261 }
mzling 0:f0dc2775ec68 262 }
mzling 0:f0dc2775ec68 263 char* checksum = get_checksum(message, len-3);
mzling 0:f0dc2775ec68 264 if (checksum[0] != message[len-3]) {
mzling 0:f0dc2775ec68 265 //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
mzling 0:f0dc2775ec68 266 free(checksum);
mzling 0:f0dc2775ec68 267 // send_error(CHECKSUM_ERR);
mzling 0:f0dc2775ec68 268 return false;
mzling 0:f0dc2775ec68 269 }
mzling 0:f0dc2775ec68 270 if (checksum[1] != message[len-2]) {
mzling 0:f0dc2775ec68 271 //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
mzling 0:f0dc2775ec68 272 free(checksum);
mzling 0:f0dc2775ec68 273 // send_error(CHECKSUM_ERR);
mzling 0:f0dc2775ec68 274 return false;
mzling 0:f0dc2775ec68 275 }
mzling 0:f0dc2775ec68 276 if (checksum[2] != message[len-1]) {
mzling 0:f0dc2775ec68 277 //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
mzling 0:f0dc2775ec68 278 free(checksum);
mzling 0:f0dc2775ec68 279 // send_error(CHECKSUM_ERR);
mzling 0:f0dc2775ec68 280 return false;
mzling 0:f0dc2775ec68 281 }
mzling 0:f0dc2775ec68 282 free(checksum);
mzling 0:f0dc2775ec68 283
mzling 0:f0dc2775ec68 284 return true;
mzling 0:f0dc2775ec68 285 }
mzling 0:f0dc2775ec68 286
mzling 0:f0dc2775ec68 287
mzling 0:f0dc2775ec68 288
mzling 0:f0dc2775ec68 289 /**
mzling 0:f0dc2775ec68 290 * Checks received WRITE message and writes to paramMap/SDCard
mzling 0:f0dc2775ec68 291 * @param message The received WRITE message
mzling 0:f0dc2775ec68 292 * @param len Length of the WRITE message
mzling 0:f0dc2775ec68 293 * @author Michael Ling
mzling 0:f0dc2775ec68 294 * @date 2/4/2015
mzling 0:f0dc2775ec68 295 */
mzling 0:f0dc2775ec68 296 void dataComm::process_write(short int* msg, int len)
mzling 0:f0dc2775ec68 297 //void dataComm::process_write(char *msg, int len)
mzling 0:f0dc2775ec68 298 {
mzling 2:53547eb587fb 299
mzling 0:f0dc2775ec68 300 if (msg[0] == 0) {
mzling 0:f0dc2775ec68 301 return;
mzling 0:f0dc2775ec68 302 }
mzling 2:53547eb587fb 303 //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone.
mzling 0:f0dc2775ec68 304 char *message = (char*) msg;
mzling 0:f0dc2775ec68 305 /*
mzling 0:f0dc2775ec68 306 for (int i = 0; i < len*2; i += 2) {
mzling 0:f0dc2775ec68 307 message[i] = msg[i/2] & 0xff;
mzling 0:f0dc2775ec68 308 message[i+1] = (msg[i/2] >> 8) & 0xff;
mzling 0:f0dc2775ec68 309 }
mzling 0:f0dc2775ec68 310 for (int i = 0; i < len*2; i+=1) {
mzling 0:f0dc2775ec68 311 //if (message[i] != 0) {
mzling 0:f0dc2775ec68 312 pc.printf("%x, ",message[i]);
mzling 0:f0dc2775ec68 313 //}
mzling 0:f0dc2775ec68 314 }
mzling 0:f0dc2775ec68 315 pc.printf("\r\n");*/
mzling 0:f0dc2775ec68 316
mzling 0:f0dc2775ec68 317 /*
mzling 0:f0dc2775ec68 318 char *message = convert_to_char_array(msg, len);
mzling 0:f0dc2775ec68 319 if (message[0] == 0xff) {
mzling 0:f0dc2775ec68 320
mzling 0:f0dc2775ec68 321 char message[len/2];
mzling 0:f0dc2775ec68 322 for (int i = 4; i < len; i += 4) {
mzling 0:f0dc2775ec68 323 message[(i-4)/2] = msg[i];
mzling 0:f0dc2775ec68 324 message[(i-4)/2+1] = msg[i+1];
mzling 0:f0dc2775ec68 325 }*/
mzling 0:f0dc2775ec68 326 /*
mzling 0:f0dc2775ec68 327 for (int j=0;j<len;j+=1){
mzling 0:f0dc2775ec68 328 //if (message[j] != 0) {
mzling 0:f0dc2775ec68 329 printf("%d: %x\r\n", j, message[j]);
mzling 0:f0dc2775ec68 330 //}
mzling 0:f0dc2775ec68 331 }
mzling 0:f0dc2775ec68 332 //}
mzling 0:f0dc2775ec68 333 mbedLED1 = 1;
mzling 0:f0dc2775ec68 334 /*
mzling 0:f0dc2775ec68 335 if (message[2] == END) {
mzling 0:f0dc2775ec68 336 _failures += 1;
mzling 0:f0dc2775ec68 337 if (_failures < 5) {
mzling 0:f0dc2775ec68 338 send(_lastCmd);
mzling 0:f0dc2775ec68 339 } else {
mzling 0:f0dc2775ec68 340 _failures = 0;
mzling 0:f0dc2775ec68 341 }
mzling 0:f0dc2775ec68 342 printf("Exited\r\n");
mzling 0:f0dc2775ec68 343
mzling 0:f0dc2775ec68 344 return;
mzling 0:f0dc2775ec68 345 }
mzling 0:f0dc2775ec68 346
mzling 0:f0dc2775ec68 347 */
mzling 0:f0dc2775ec68 348
mzling 0:f0dc2775ec68 349 if (!msg_check(message, len*2)) {
mzling 0:f0dc2775ec68 350 return;
mzling 0:f0dc2775ec68 351 }
mzling 0:f0dc2775ec68 352 mbedLED2 = 1;
mzling 2:53547eb587fb 353 //If the msgCheck passes, we change the local paramMap and set variables to their new values
mzling 0:f0dc2775ec68 354 for (int i=2; i < len*2-5; i+=2) {
mzling 0:f0dc2775ec68 355 //printf("Loop %d\r\n", i);
mzling 0:f0dc2775ec68 356 if (message[i] == END) {
mzling 0:f0dc2775ec68 357 return;
mzling 0:f0dc2775ec68 358 }
mzling 0:f0dc2775ec68 359 char msgc = message[i] & 0xbf;
mzling 0:f0dc2775ec68 360 if ((msgc & 0x80) != 0x80) {
mzling 0:f0dc2775ec68 361 return;
mzling 0:f0dc2775ec68 362 }
mzling 0:f0dc2775ec68 363 int index = msgc & 0x7f;
mzling 0:f0dc2775ec68 364
mzling 0:f0dc2775ec68 365 _paramMap[_indexMap[index]] = message[i+1];
mzling 0:f0dc2775ec68 366 generic_set(_indexMap[index], message[i+1]);
mzling 2:53547eb587fb 367
mzling 0:f0dc2775ec68 368 }
mzling 0:f0dc2775ec68 369 }