static void TURN_PrintStatus(const CLS1_StdIOType *io) { unsigned char buf[32]; CLS1_SendStatusStr((unsigned char*)"turn", (unsigned char*)"\r\n", io->stdOut); #if PL_HAS_QUADRATURE UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Steps90); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n"); CLS1_SendStatusStr((unsigned char*)" steps90", buf, io->stdOut); UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepsFwBw); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n"); CLS1_SendStatusStr((unsigned char*)" stepsfw", buf, io->stdOut); UTIL1_Num16sToStr(buf, sizeof(buf), Q4CLeft_GetPos()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", "); UTIL1_strcatNum16u(buf, sizeof(buf), Q4CLeft_NofErrors()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n"); CLS1_SendStatusStr((unsigned char*)" left pos", buf, io->stdOut); UTIL1_Num16sToStr(buf, sizeof(buf), Q4CRight_GetPos()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", "); UTIL1_strcatNum16u(buf, sizeof(buf), Q4CRight_NofErrors()); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n"); CLS1_SendStatusStr((unsigned char*)" right pos", buf, io->stdOut); #else UTIL1_Num16uToStr(buf, sizeof(buf), TURN_DutyPercent); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n"); CLS1_SendStatusStr((unsigned char*)" duty", buf, io->stdOut); UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Time90ms); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n"); CLS1_SendStatusStr((unsigned char*)" time90", buf, io->stdOut); UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepFwBwMs); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n"); CLS1_SendStatusStr((unsigned char*)" timefw", buf, io->stdOut); #endif }
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 */ }