add command incSpeed

ADIS_Sumo_Styger/Sumo/Drive.c

@@ -258,6 +258,7 @@ static void DRV_PrintHelp(const McuShell_StdIOType *io) {
   McuShell_SendHelpStr((unsigned char*)"  mode <mode>", (unsigned char*)"Set driving mode (none|stop|speed)\r\n", io->stdOut);
 #endif
   McuShell_SendHelpStr((unsigned char*)"  speed <left> <right>", (unsigned char*)"Move left and right motors with given speed\r\n", io->stdOut);
+  McuShell_SendHelpStr((unsigned char*)"  incSpeed <left> <right>", (unsigned char*)"Increase left and right motors with given speed\r\n", io->stdOut);
 #if PL_CONFIG_USE_POS_PID
   McuShell_SendHelpStr((unsigned char*)"  pos <left> <right>", (unsigned char*)"Move left and right wheels to given position\r\n", io->stdOut);
   McuShell_SendHelpStr((unsigned char*)"  pos reset", (unsigned char*)"Reset drive and wheel position\r\n", io->stdOut);
@@ -329,6 +330,23 @@ uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell
       McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
       res = ERR_FAILED;
     }
+  }else if (McuUtility_strncmp((char*)cmd, (char*)"drive incSpeed ", sizeof("drive incSpeed ")-1)==0) {
+	    p = cmd+sizeof("drive incSpeed");
+	    if (McuUtility_xatoi(&p, &val1)==ERR_OK) {
+	      if (McuUtility_xatoi(&p, &val2)==ERR_OK) {
+	        int32_t leftSpeed = DRV_Status.speed.left+val1;
+	        int32_t rightSpeed = DRV_Status.speed.right+val2;
+	    	  if (DRV_SetSpeed(leftSpeed, rightSpeed)!=ERR_OK) {
+	          McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
+	        }
+	        *handled = TRUE;
+	      } else {
+	        McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr);
+	      }
+	    } else {
+	      McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
+	      res = ERR_FAILED;
+	    }
 	#if PL_CONFIG_USE_POS_PID
   } else if (McuUtility_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
     QuadCounter_SetPosLeft(0);