Example #1
0
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  int32_t val;
  const unsigned char *p;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor help")==0) {
    MOT_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor status")==0) {
    MOT_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L forward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R forward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor both go")==0) {
	    MOT_StartMotors();
	    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor both fw")==0) {
	    MOT_DriveForward();
	    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor both bw")==0) {
	    MOT_DriveBackward();
	    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor stop")==0) {
	    MOT_StopMotors();
	    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L backward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R backward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
    p = cmd+sizeof("motor L duty");
    if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
      MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
      *handled = TRUE;
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
    p = cmd+sizeof("motor R duty");
    if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
      MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
      *handled = TRUE;
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  }
  return res;
}
Example #2
0
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  long val;
  const unsigned char *p;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor help")==0) {
    MOT_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor status")==0) {
    MOT_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L forward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R forward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L backward")==0) {
    MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R backward")==0) {
    MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
    if (!isMotorOn) {
      CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
      res = ERR_FAILED;
    } else {
      p = cmd+sizeof("motor L duty");
      if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
        MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
        res = ERR_FAILED;
      }
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
    if (!isMotorOn) {
      CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
      res = ERR_FAILED;
    } else {
      p = cmd+sizeof("motor R duty");
      if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
        MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
        res = ERR_FAILED;
      }
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor on", sizeof("motor on")-1)==0) {
    isMotorOn = TRUE;
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"motor off", sizeof("motor off")-1)==0) {
    MOT_SetSpeedPercent(&motorL, 0);
    MOT_SetSpeedPercent(&motorR, 0);
    isMotorOn = FALSE;
    *handled = TRUE;
#if PL_HAS_MOTOR_BRAKE
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake on")==0) {
    MOT_SetBrake(&motorL, TRUE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake off")==0) {
    MOT_SetBrake(&motorL, FALSE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake on")==0) {
    MOT_SetBrake(&motorR, TRUE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake off")==0) {
    MOT_SetBrake(&motorR, FALSE);
    *handled = TRUE;
#endif
  }
  return res;
}