void DRV_Init(void) { DRV_EnableDisable(FALSE); DRV_SpeedLeft = 0; DRV_SpeedRight = 0; DRV_EnableDisablePos(FALSE); DRV_PosLeft = 30000; DRV_PosRight = 30000; Q4CLeft_SetPos(30000); Q4CRight_SetPos(30000); if (FRTOS1_xTaskCreate( DriveTask, /* pointer to the task */ "Drive", /* task name for kernel awareness debugging */ configMINIMAL_STACK_SIZE, /* task stack size */ (void*)NULL, /* optional task startup argument */ tskIDLE_PRIORITY, /* initial priority */ (xTaskHandle*)NULL /* optional task handle to create */ ) != pdPASS) { /*lint -e527 */ for(;;){} /* error! probably out of memory */ /*lint +e527 */ } }
static portTASK_FUNCTION(DriveTask, pvParameters) { (void)pvParameters; /* parameter not used */ bool prevOnS, prevOnP; uint32_t i; int32_t PosL,PosR; uint32_t dL,dR; uint32_t sL,sR; //traceLabel usrEvent; prevOnS = DRV_SpeedOn; prevOnP = DRV_PosOn; sL = 30000; sR = 30000; #if PL_HAS_RTOS_TRACE //usrEvent = xTraceOpenLabel("drive"); #endif for(;;) { #if PL_HAS_RTOS_TRACE //RTOSTRC1_vTraceUserEvent(usrEvent); #endif /*! \todo extend this for your own needs and with a position PID */ TACHO_CalcSpeed(); if (prevOnS && !DRV_SpeedOn) { /* speed turned off */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); PID_Start(); /* reset values */ } else if (DRV_SpeedOn && !DRV_PosOn) { /*speed control*/ PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */ PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */ } else if (DRV_PosOn && !DRV_SpeedOn) { /*pos control*/ PosL = Q4CLeft_GetPos(); PosR = Q4CRight_GetPos(); dL = ((PosL-DRV_PosLeft)<=0) ? -(PosL-DRV_PosLeft) : (PosL-DRV_PosLeft); dR = ((PosR-DRV_PosRight)<=0) ? -(PosR-DRV_PosRight) : (PosR-DRV_PosRight); if(dL>100){ if((DRV_PosLeft-PosL)>0){ sL += 15; } else{ sL -= 15; } } else{ sL = DRV_PosLeft; } if(dR>100){ if((DRV_PosRight-PosR)>0){ sR += 15; } else{ sR -= 15; } } else{ sR = DRV_PosRight; } PID_Pos(PosL, sL, TRUE); //left PID_Pos(PosR, sR, FALSE); //right if((PosR < 1000) || (PosR > 63000)){ /*range safety*/ DRV_PosRight = 30000; Q4CRight_SetPos(30000); } if((PosL < 1000) || (PosL > 63000)){ DRV_PosLeft = 30000; Q4CLeft_SetPos(30000); } } else if(prevOnP && !DRV_PosOn){ /*pos turned off*/ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); for(i=0;i<100000;i++); DRV_PosLeft = 30000; DRV_PosRight = 30000; Q4CLeft_SetPos(30000); Q4CRight_SetPos(30000); } prevOnS = DRV_SpeedOn; prevOnP = DRV_PosOn; FRTOS1_vTaskDelay(2/portTICK_RATE_MS); } /* for */ }
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; }