changed to be compatible with struct

Fork of dataComm by Michael Ling

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers dataComm.cpp Source File

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 }