static void ESC_PrintESCStatus(ESC_MotorID id, const CLS1_StdIOType *io) { unsigned char buf[48]; const unsigned char *motorStr; if (id==ESC_MOTOR_FRONT_RIGHT) { motorStr = " fr"; } else if (id==ESC_MOTOR_FRONT_LEFT) { motorStr = " fl"; } else if (id==ESC_MOTOR_BACK_LEFT) { motorStr = " bl"; } else if (id==ESC_MOTOR_BACK_RIGHT) { motorStr = " br"; } else { motorStr = " ??"; } CLS1_SendStatusStr(motorStr, (unsigned char*)"", io->stdOut); buf[0] = '\0'; UTIL1_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)ESC_motors[id].currSpeedPercent, ' ', 4); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"% 0x"); UTIL1_strcatNum16Hex(buf, sizeof(buf), ESC_motors[id].currPWMvalue); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", "); UTIL1_strcatNum16u(buf, sizeof(buf), ESC_motors[id].us); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" us\r\n"); CLS1_SendStr(buf, io->stdOut); }
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); }