static void MOT_PrintStatus(const CLS1_StdIOType *io) { unsigned char buf[32]; CLS1_SendStatusStr((unsigned char*)"Motor", (unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" on/off", isMotorOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" motor L", (unsigned char*)"", io->stdOut); buf[0] = '\0'; UTIL1_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorL.currSpeedPercent, ' ', 4); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"% 0x"); UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorL)); UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorL)==MOT_DIR_FORWARD?", fw":", bw")); #if PL_HAS_MOTOR_BRAKE UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorL)?", brake on":", brake off")); #endif CLS1_SendStr(buf, io->stdOut); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" motor R", (unsigned char*)"", io->stdOut); buf[0] = '\0'; UTIL1_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorR.currSpeedPercent, ' ', 4); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"% 0x"); UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorR)); UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorR)==MOT_DIR_FORWARD?", fw":", bw")); #if PL_HAS_MOTOR_BRAKE UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorR)?", brake on":", brake off")); #endif CLS1_SendStr(buf, io->stdOut); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); #if PL_HAS_MOTOR_CURRENT_SENSE WriteCurrent(&motorA, io); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); WriteCurrent(&motorB, io); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); #endif }
static void MOT_PrintStatus(const CLS1_StdIOType *io) { unsigned char buf[32]; CLS1_SendStatusStr((unsigned char*)"Motor", (unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" motor L", (unsigned char*)"", io->stdOut); buf[0] = '\0'; UTIL1_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorL.currSpeedPercent, ' ', 4); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"% 0x"); UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorL)); UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorL)==MOT_DIR_FORWARD?", fw":", bw")); CLS1_SendStr(buf, io->stdOut); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); CLS1_SendStatusStr((unsigned char*)" motor R", (unsigned char*)"", io->stdOut); buf[0] = '\0'; UTIL1_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorR.currSpeedPercent, ' ', 4); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"% 0x"); UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorR)); UTIL1_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorR)==MOT_DIR_FORWARD?", fw":", bw")); CLS1_SendStr(buf, io->stdOut); CLS1_SendStr((unsigned char*)"\r\n", io->stdOut); }
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 */ }