changed to be compatible with struct
Fork of dataComm by
Embed:
(wiki syntax)
Show/hide line numbers
dataComm.cpp
00001 #include "mbed.h" 00002 00003 #include "dataComm.h" 00004 #include "initExoVars.h" 00005 #include "gaitGenerator.h" 00006 #include <string> 00007 #include <map> 00008 00009 //Much code is shared with BluetoothComm class, on the control bed 00010 dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8) 00011 { 00012 mbedLED1 = 1; 00013 int temp1[] = {0,1,2,3,4,8,9,10,-1}; 00014 00015 for (int i = 0; i < _escapesNeeded+1; i++) { 00016 _escapeNeeded[i] = temp1[i]; 00017 } 00018 00019 00020 std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing", 00021 "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle", 00022 "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle", 00023 "StepLength", "StepTime", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd", 00024 "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle", 00025 "RHipRefAngle", "Charge"}; 00026 //Populate the map of indices to param names 00027 for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) { 00028 _indexMap[j] = temp2[j]; 00029 } 00030 00031 for (int j = 0; j < _numVars; j += 1) { 00032 _paramMap[_indexMap[j]] = generic_get(_indexMap[j]); 00033 } 00034 00035 // pc.printf("Initialized PARAM \r\n"); 00036 int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10}; 00037 00038 for (int k = 0; k < _numReadOnlyParams; k += 1) { 00039 _paramMap[_indexMap[_numVars + k]] = temp4[k]; 00040 } 00041 //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]); 00042 00043 } 00044 00045 /** 00046 * Returns the stored value of the specified variable 00047 * @param var The variable to be gotten (as a string). 00048 * @author Michael Ling 00049 * @date 3/31/2015 00050 */ 00051 short dataComm::generic_get(std::string var) 00052 { 00053 //Translate the raw value of the parameter to a short between 0-100 00054 if (var.compare("SittingAngle") == 0) { 00055 return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100); 00056 } else if(var.compare( "BentForwardAngle")==0) { 00057 return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100); 00058 } else if (var.compare("StandupAsst")==0) { 00059 return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100); 00060 } else if (var.compare("SitdownAsst")==0) { 00061 return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100); 00062 } else if (var.compare("SitdownTime")==0) { 00063 return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100); 00064 } else if (var.compare( "StandingAngle")==0) { 00065 return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100); 00066 } else if (var.compare("WalkAngle")==0) { 00067 return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); 00068 } else if (var.compare("StepTime")==0) { 00069 return (short) ((mm_gait_params.time_steps-MIN_STEPTIME)/(MAX_STEPTIME-MIN_STEPTIME)*100); 00070 } else if (var.compare("PhaseShift") == 0) { 00071 return (short) ((mm_gait_params.peak_time-MIN_PHASESHIFT)/(MAX_PHASESHIFT-MIN_PHASESHIFT)*100); 00072 } else if (var.compare("WalkAngle") == 0) { 00073 return (short) ((mm_gait_params.walking_angle-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); 00074 } else if (var.compare("StepLength") == 0) { 00075 return (short) ((mm_gait_params.end_angle-MIN_STEPLEN)/(MAX_STEPLEN-MIN_STEPLEN)*100); 00076 } else if (var.compare("HipFlex") == 0) { 00077 return (short) ((mm_gait_params.max_angle-MIN_HIPFLEX)/(MAX_HIPFLEX-MIN_HIPFLEX)*100); 00078 } 00079 return 0; 00080 00081 } 00082 00083 /** 00084 * Sets a new value for the given variable 00085 * @param var The variable to be set, as a string 00086 * @param newval New value for the variable 00087 * @author Michael Ling 00088 * @date 3/31/2015 00089 */ 00090 void dataComm::generic_set(std::string var, short newval) 00091 { 00092 mbedLED3 = 1; 00093 //newval is a short from 0-100, and needs to be converted to a raw value. 00094 //We do this by making a 100-point scale between MIN and MAX param values 00095 if (var.compare( "SittingAngle")==0) { 00096 sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT); 00097 // pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100)); 00098 } else if (var.compare( "BentForwardAngle")==0) { 00099 bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT); 00100 // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100)); 00101 } else if (var.compare("StandupAsst")==0) { 00102 fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST)); 00103 // pc.printf("%d\r\n", (short)fsm.get_standup_asst()); 00104 } else if (var.compare("SitdownAsst")==0) { 00105 fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST)); 00106 // pc.printf("%d\r\n", (short)fsm.get_sitdown_asst()); 00107 } else if (var.compare("SitdownTime")==0) { 00108 tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME); 00109 // pc.printf("%d\r\n", (short)tSittingDown); 00110 } else if (var.compare("StandingAngle")==0) { 00111 //pc.printf("LED\r\n"); 00112 motorLED = 1; 00113 stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND); 00114 encoder_L.init(zero_ang_L+stand_adjust); 00115 encoder_R.init(zero_ang_R+stand_adjust); 00116 //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100)); 00117 } else if (var.compare("WalkAngle")==0) { 00118 fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK)); 00119 // pc.printf("%d\r\n", (short)fsm.get_backbias()); 00120 } else if (var.compare("StepTime")==0) { 00121 mm_gait_params.time_steps = MIN_STEPTIME+(float)newval/100*(MAX_STEPTIME-MIN_STEPTIME); 00122 } else if (var.compare("PhaseShift") == 0) { 00123 mm_gait_params.peak_time = MIN_PHASESHIFT+(float)newval/100*(MAX_PHASESHIFT-MIN_PHASESHIFT); 00124 } else if (var.compare("WalkAngle") == 0) { 00125 mm_gait_params.walking_angle = MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK); 00126 } else if (var.compare("StepLength") == 0) { 00127 mm_gait_params.end_angle = MIN_STEPLEN+(float)newval/100*(MAX_STEPLEN-MIN_STEPLEN); 00128 } else if (var.compare("HipFlex") == 0) { 00129 mm_gait_params.max_angle = MIN_HIPFLEX+(float)newval/100*(MAX_HIPFLEX-MIN_HIPFLEX); 00130 } 00131 //MORE else blocks for gait params 00132 00133 } 00134 /** 00135 * Calculates parity--0 if c even, 1 if odd 00136 * @param c The short that we want to calculate parity of 00137 * @author Michael Ling 00138 * @date 2/4/2015 00139 */ 00140 bool dataComm::parity(short c) 00141 { 00142 bool p = false; 00143 while (c != 0) { 00144 p = !p; 00145 c = (short) (c & (c-1)); 00146 } 00147 return p; 00148 } 00149 00150 /** 00151 * Calculates checksum of char array, sum of all chars in array 00152 * @param b The char array to be summed up 00153 * @param len Length of the array b 00154 * @author Michael Ling 00155 * @date 2/4/2015 00156 */ 00157 char* dataComm::get_checksum(char* b, int len) 00158 { 00159 char* checksum = (char*)malloc(3*sizeof(char)); 00160 short sum = 0; 00161 for (int i = 0; i < len; i += 1) { 00162 sum += ((short) b[i] & 0xff); 00163 } 00164 checksum[1] = (char) ((sum >> 8) & 0xff); 00165 checksum[2] = (char) (sum & 0xff); 00166 char check = (char) (sum & 0xff); 00167 //Sets escape character if the checksum contains START or END 00168 if (check == START || check == END) { 00169 checksum[0] = 1; 00170 checksum[2] = (char) (checksum[2] & 0x1); 00171 } else { 00172 checksum[0] = 0; 00173 } 00174 return checksum; 00175 } 00176 00177 00178 00179 /** 00180 * 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 00181 * @param message The received message to check 00182 * @param len Length of the message 00183 * @author Michael Ling 00184 * @date 2/4/2015 00185 */ 00186 bool dataComm::msg_check(char* message, int len) 00187 { 00188 //Checks the message starts with 0xff 00189 if (message[0] != START) { 00190 //printf("Improper START or END \r\n"); 00191 00192 return false; 00193 } 00194 //printf("got a start\r\n"); 00195 //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END 00196 //Otherwise, the msg_check procedure is the same 00197 while (message[len-4] != END && len >= 6) { 00198 len -= 1; 00199 } 00200 if (message[len-4] != END) { 00201 //send_error(END_ERR); 00202 return false; 00203 } 00204 bool write = message[2] & 0x80; 00205 for (int i=2; i < len-5; i+=2) { 00206 if (i == 2 && message[i] == READONLY_IND) { 00207 break; 00208 } 00209 if (i == (len-5) && message[i] == READONLY_IND) { 00210 break; 00211 } 00212 if (((message[i] & 0x80) !=0) && !write) { 00213 //printf("Does not match READ format \r\n"); 00214 return false; 00215 } 00216 if (((message[i] & 0x80) != 0x80) && write) { 00217 //printf("char %x Does not match WRITE format \r\n", message[i]); 00218 return false; 00219 } 00220 short s; 00221 if (write) { 00222 s = (short) ((message[i] << 8) | message[i+1]); 00223 } else { 00224 s = (short) (message[i] << 8); 00225 } 00226 //Checks the parity bit of the var/data pair. 00227 bool parity1 = (s & 0x4000) != 0; 00228 00229 if (parity1 != parity(s & 0x4000)) { 00230 //printf("Parity error in VAR char \r\n"); 00231 return false; 00232 } 00233 00234 char c = message[i] & 0x3f; 00235 char c2 = message[i+1]; 00236 //Makes sure the var is a valid one 00237 if ((int) c < 0 || (int) c > _numVars) { 00238 //printf("VAR char out of range \r\n"); 00239 return false; 00240 } 00241 //Makes sure the new value is in the range 0-100 00242 if ((int) c2 < 0 || (int) c2 > 100) { 00243 //printf("DATA char out of range"); 00244 return false; 00245 } 00246 } 00247 char* checksum = get_checksum(message, len-3); 00248 if (checksum[0] != message[len-3]) { 00249 //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]); 00250 free(checksum); 00251 return false; 00252 } 00253 if (checksum[1] != message[len-2]) { 00254 //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]); 00255 free(checksum); 00256 return false; 00257 } 00258 if (checksum[2] != message[len-1]) { 00259 //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]); 00260 free(checksum); 00261 return false; 00262 } 00263 free(checksum); 00264 00265 return true; 00266 } 00267 00268 00269 00270 /** 00271 * Checks received WRITE message and writes to paramMap/SDCard 00272 * @param message The received WRITE message 00273 * @param len Length of the WRITE message 00274 * @author Michael Ling 00275 * @date 2/4/2015 00276 */ 00277 void dataComm::process_write(short int* msg, int len) 00278 { 00279 00280 if (msg[0] == 0) { 00281 return; 00282 } 00283 //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone. 00284 char *message = (char*) msg; 00285 00286 00287 if (!msg_check(message, len*2)) { 00288 return; 00289 } 00290 mbedLED2 = 1; 00291 //If the msgCheck passes, we change the local paramMap and set variables to their new values 00292 for (int i=2; i < len*2-5; i+=2) { 00293 //printf("Loop %d\r\n", i); 00294 if (message[i] == END) { 00295 return; 00296 } 00297 char msgc = message[i] & 0xbf; 00298 if ((msgc & 0x80) != 0x80) { 00299 return; 00300 } 00301 int index = msgc & 0x7f; 00302 00303 _paramMap[_indexMap[index]] = message[i+1]; 00304 generic_set(_indexMap[index], message[i+1]); 00305 00306 } 00307 }
Generated on Wed Jul 13 2022 15:23:41 by
1.7.2
