static void PrintPIDstatus(PID_Config *config, const unsigned char *kindStr, const CLS1_StdIOType *io) {
unsigned char buf[48];
unsigned char kindBuf[16];
UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" PID");
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"p: ");
UTIL1_strcatNum32s(buf, sizeof(buf), config->pFactor100);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" i: ");
UTIL1_strcatNum32s(buf, sizeof(buf), config->iFactor100);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" d: ");
UTIL1_strcatNum32s(buf, sizeof(buf), config->dFactor100);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr(kindBuf, buf, io->stdOut);
UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" windup");
UTIL1_Num32sToStr(buf, sizeof(buf), config->iAntiWindup);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr(kindBuf, buf, io->stdOut);
UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)" ");
UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" speed");
UTIL1_Num8uToStr(buf, sizeof(buf), config->maxSpeedPercent);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
CLS1_SendStatusStr(kindBuf, buf, io->stdOut);
}
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);
}
static void DRV_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[32];
CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" speed", DRV_SpeedOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedLeft);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" speed L", buf, io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedRight);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" speed R", buf, io->stdOut);
}
