From c70abf71bca34aadf0850fbdd6b9b2bac969d221 Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Wed, 7 Nov 2018 12:03:58 -0800 Subject: [PATCH] Add checks for bad data, file handle. Dont move servos if Joint 6 / 7 value is "NaN" Check the result of ProcessServerReceiveDataDDE and don't try to interpret the data if it returned a false. This keeps DexRun from crashing if invalid data is sent. e.g. missing the ';' at the EOL. Additional checks on file handle wfp to verify it's an open file (greater than 0) before attempting to close it, always set to zero when closed. (vs -1) Previously we were checking if wfp was equal to zero, and was sometimes be set to -1 when the file was closed. See: https://github.com/HaddingtonDynamics/Dexter/commit/8976cc6132564b9ba65c3139c301465ad49aa019#diff-691272021fae98368efb598f8e089c16 If a user sends an 'a' command with 5 numbers, only the first 5 joints will be moved, Joints 6 and 7 (the Dynamixel servos) will not be sent any command. The same is true for joint 7 only if you send 6 numbers; joint 6 will move, joint 7 won't. In order to support moving joint 7 and not moving joint 6, the option to send "NaN" is supported for joint 6 and 7. E.g. "a 0 0 0 0 0 NaN 0" will move all joints, except joint 6, to home. In fact, the code is only checking the first letter of the value, so anything that starts with a capital "N" will do. --- Firmware/DexRun.c | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/Firmware/DexRun.c b/Firmware/DexRun.c index d5020c9..44ad599 100644 --- a/Firmware/DexRun.c +++ b/Firmware/DexRun.c @@ -2357,10 +2357,10 @@ void *StartServerSocketDDE(void *arg) // while ( (RLength = recv (connfd,recBuff,sizeof(recBuff),0 )) > 0) { //recBuff[RLength]=0; - (void)ProcessServerReceiveDataDDE(recBuff); - - (void)ProcessServerSendDataDDE(sendBuff,recBuff);/*==TRUE)*/ + if (ProcessServerReceiveDataDDE(recBuff)) { + (void)ProcessServerSendDataDDE(sendBuff,recBuff);/*==TRUE)*/ write (connfd,sendBuff,60*4/*sizeof(sendBuff)*/); + } } } //printf("error code %s \n",strerror(errno)); @@ -4439,7 +4439,6 @@ int ParseInput(char *iString) int d3,d4,d5; float f1; ////printf("\nStart wait Goal"); - //printf("\n\nReceived String: %s\n", iString); if(iString !=NULL) { token = strtok (iString, delimiters); @@ -4456,7 +4455,7 @@ int ParseInput(char *iString) case WRITE_TO_ROBOT: p1=strtok (NULL, delimiters); Add=(int)p1[0]; - //printf("\nwrite %s %d: ",p1,Add); + //printf("write %s %d: \n",p1,Add); switch(Add) { case 'f': //filename p2=strtok(NULL, delimiters);//always zero, toss it. @@ -4479,7 +4478,7 @@ int ParseInput(char *iString) i=fwrite(p3, 1, Length, wfp); //printf("Wrote %d bytes. ",i); } - if('e'==Add && wfp) { + if('e'==Add && (wfp>0)) { fclose(wfp); wfp = 0; printf("...Finished writing.\n"); @@ -4790,11 +4789,11 @@ int ParseInput(char *iString) p5=strtok (NULL, delimiters); p6=strtok (NULL, delimiters); - if (p6 && 'x'!=p6[0]) SetGripperRoll(atoi(p6)); + if (p6 && 'N'!=p6[0]) SetGripperRoll(atoi(p6)); //if(p6 != NULL){ printf("p6 %s\n",p6); } //else{ printf("p6 doesn't exist\n"); } p7=strtok (NULL, delimiters); - if (p7 && 'x'!=p7[0]) SetGripperSpan(atoi(p7)); + if (p7 && 'N'!=p7[0]) SetGripperSpan(atoi(p7)); //if(p7 != NULL){ printf("p7 %s\n",p7); } //else{ printf("p7 doesn't exist\n"); } @@ -5180,7 +5179,6 @@ int main(int argc, char *argv[]) { int ip_b = 0; int ip_c = 0; const char delimiters[] = " .\t"; - if (wfp>0) {fclose(wfp); wfp = 0;} wfp = fopen("/etc/network/interfaces", "r"); while(fgets(iString, ISTRING_LEN, wfp) != NULL && i < 20) { if((strstr(iString, "address 192.168.")) != NULL) { @@ -5245,7 +5243,7 @@ int main(int argc, char *argv[]) { i++; } - + if (wfp>0) {fclose(wfp); wfp = 0;} /* wfp = fopen("/etc/network/interfaces", "r"); token = strtok ((char *)wfp, delimiters);