static portTASK_FUNCTION(DriveTask, pvParameters) { (void)pvParameters; /* parameter not used */ bool prevOn; //traceLabel usrEvent; prevOn = DRV_SpeedOn; #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 (prevOn && !DRV_SpeedOn) { /* 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) { PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */ PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */ } prevOn = DRV_SpeedOn; FRTOS1_vTaskDelay(2/portTICK_RATE_MS); } /* for */ }
static void DriveTask(void *pvParameters) { portTickType xLastWakeTime; (void)pvParameters; xLastWakeTime = xTaskGetTickCount(); for(;;) { while (GetCmd()==ERR_OK) { /* returns ERR_RXEMPTY if queue is empty */ /* process incoming commands */ } TACHO_CalcSpeed(); if (DRV_Status.mode==DRV_MODE_SPEED) { PID_Speed(TACHO_GetSpeed(TRUE), DRV_Status.speed.left, TRUE); PID_Speed(TACHO_GetSpeed(FALSE), DRV_Status.speed.right, FALSE); } else if (DRV_Status.mode==DRV_MODE_STOP) { PID_Speed(TACHO_GetSpeed(TRUE), 0, TRUE); PID_Speed(TACHO_GetSpeed(FALSE), 0, FALSE); } else if (DRV_Status.mode==DRV_MODE_POS) { PID_Pos(Q4CLeft_GetPos(), DRV_Status.pos.left, TRUE); PID_Pos(Q4CRight_GetPos(), DRV_Status.pos.right, FALSE); } else if (DRV_Status.mode==DRV_MODE_NONE) { /* do nothing */ } FRTOS1_vTaskDelayUntil(&xLastWakeTime, 5/portTICK_RATE_MS); } /* for */ }
static void DRV_PrintStatus(const CLS1_StdIOType *io) { uint8_t buf[48]; CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" mode", DRV_GetModeStr(DRV_Status.mode), io->stdOut); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.left); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: "); UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(TRUE)); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n"); CLS1_SendStatusStr((unsigned char*)" speed left", buf, io->stdOut); UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.right); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: "); UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(FALSE)); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n"); CLS1_SendStatusStr((unsigned char*)" speed right", buf, io->stdOut); UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.left); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: "); UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CLeft_GetPos()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n"); CLS1_SendStatusStr((unsigned char*)" pos left", buf, io->stdOut); UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.right); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: "); UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CRight_GetPos()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n"); CLS1_SendStatusStr((unsigned char*)" pos right", buf, io->stdOut); }
bool DRV_HasTurned(void) { int16_t pos; if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) { return FALSE; /* still messages in command queue, so there is something pending */ } if (DRV_Status.mode==DRV_MODE_POS) { #define DRV_TURN_SPEED_LOW 50 int32_t speedL, speedR; speedL = TACHO_GetSpeed(TRUE); speedR = TACHO_GetSpeed(FALSE); if (speedL>-DRV_TURN_SPEED_LOW && speedL<DRV_TURN_SPEED_LOW && speedR>-DRV_TURN_SPEED_LOW && speedR<DRV_TURN_SPEED_LOW) { /* speed close to zero */ pos = Q4CLeft_GetPos(); if (match(pos, DRV_Status.pos.left)) { pos = Q4CRight_GetPos(); if (match(pos, DRV_Status.pos.right)) { return TRUE; } } } return FALSE; } /* if */ return TRUE; }
/*! * \brief Prints the system low power status * \param io I/O channel to use for printing status */ static void TACHO_PrintStatus(const CLS1_StdIOType *io) { CLS1_SendStatusStr((unsigned char*)"Tacho", (unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" L speed", (unsigned char*)"", io->stdOut); CLS1_SendNum32s(TACHO_GetSpeed(TRUE), io->stdOut); CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" R speed", (unsigned char*)"", io->stdOut); CLS1_SendNum32s(TACHO_GetSpeed(FALSE), io->stdOut); CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut); }
/*! * \brief Prints the system low power status * \param io I/O channel to use for printing status */ static void TACHO_PrintStatus(const CLS1_StdIOType *io) { //TACHO_CalcSpeed(); /* only temporary until this is done periodically */ CLS1_SendStatusStr((unsigned char*)"Tacho", (unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" L speed", (unsigned char*)"", io->stdOut); CLS1_SendNum32s(TACHO_GetSpeed(TRUE), io->stdOut); CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" R speed", (unsigned char*)"", io->stdOut); CLS1_SendNum32s(TACHO_GetSpeed(FALSE), io->stdOut); CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut); }
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 */ }
static portTASK_FUNCTION(TraceTask, pvParameters) { unsigned char buf[32]; (void)pvParameters; //ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */ for(;;) { if (traceChannel==TRACE_TO_NONE) { /* do nothing */ } else if (traceChannel==TRACE_TO_SHELL) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => "); #if PL_HAS_MOTOR if (traceMotor) { buf[0] = '\0'; if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) { UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x"); } else { UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x"); } UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT))); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_MOTOR_TACHO if (traceTacho) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:"); UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ' '); UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_QUADRATURE if (traceQuad) { buf[0] = '\0'; UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:"); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:"); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6); UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif #if PL_HAS_ACCEL if (traceAccel) { int16_t x, y, z; buf[0] = '\0'; ACCEL_GetValues(&x, &y, &z); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:"); UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6); UTIL1_chcat(buf, sizeof(buf), ';'); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif } #endif UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n"); #if PL_HAS_SHELL SHELL_SendString(&buf[0]); #endif FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */ } FRTOS1_vTaskDelay(20/portTICK_RATE_MS); } /* for */ }