C++ (Cpp) DRV_SetPos Examples

Example #1

File: Drive.c Project: chregubr85/42

uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  const uint8_t *p;
  int32_t val32;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) {
    DRV_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) {
    DRV_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed L", sizeof("drive speed L")-1)==0) {
    p = cmd+sizeof("drive speed L");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_SpeedLeft = val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed R", sizeof("drive speed R")-1)==0) {
    p = cmd+sizeof("drive speed R");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_SpeedRight = val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive speed on")==0) {
    DRV_EnableDisable(TRUE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive speed off")==0) {
    DRV_EnableDisable(FALSE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive pos on")==0) {
    DRV_EnableDisablePos(TRUE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive pos off")==0) {
	DRV_EnableDisablePos(FALSE);
	*handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos R", sizeof("drive pos R")-1)==0) {
    p = cmd+sizeof("drive pos R");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_PosRight += val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos L", sizeof("drive pos L")-1)==0) {
    p = cmd+sizeof("drive pos L");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_PosLeft += val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos", sizeof("drive pos")-1)==0) {
    p = cmd+sizeof("drive pos");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      //DRV_PosLeft += val32;
      //DRV_PosRight += val32;
    	DRV_SetPos(val32);
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive angle", sizeof("drive angle")-1)==0) {
    p = cmd+sizeof("drive angle");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      //DRV_PosLeft += val32;
      //DRV_PosRight += val32;
    	DRV_Turn(val32,0);
      *handled = TRUE;
    } else {
     res = ERR_FAILED;
    }
  }
  return res;
}

Example #2

File: Drive.c Project: sisem/intro

uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  const unsigned char *p;
  int32_t val1, val2;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) {
    DRV_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) {
    DRV_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed ", sizeof("drive speed ")-1)==0) {
    p = cmd+sizeof("drive speed");
    if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
      if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
        if (DRV_SetSpeed(val1, val2)!=ERR_OK) {
          CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
        }
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
      }
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
    Q4CLeft_SetPos(0);
    Q4CRight_SetPos(0);
    if (DRV_SetPos(0, 0)!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
    }
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) {
    p = cmd+sizeof("drive pos");
    if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
      if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
        if (DRV_SetPos(val1, val2)!=ERR_OK) {
          CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
        }
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
      }
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) {
    p = cmd+sizeof("drive mode");
    if (UTIL1_strcmp((char*)p, (char*)"none")==0) {
      if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"stop")==0) {
      if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"speed")==0) {
      if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"pos")==0) {
      if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else {
      res = ERR_FAILED;
    }
    if (res!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
    }
    *handled = TRUE;
  }
  return res;
}