uint8_t ESP_SetServer(bool startIt, uint16_t port, const CLS1_StdIOType *io, uint16_t timeoutMs) {
/* AT+CIPSERVER=<en>,<port>, where <en>: 0: stop, 1: start */
uint8_t res;
uint8_t cmd[sizeof("AT+CIPSERVER=1,80\r\n\r\nOK\r\n")+sizeof("no change")];
uint8_t rxBuf[sizeof("AT+CIPSERVER=1,80\r\n\r\nOK\r\n")+sizeof("no change")];
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSERVER=");
if (startIt) {
UTIL1_strcat(cmd, sizeof(cmd), "1,");
} else {
UTIL1_strcat(cmd, sizeof(cmd), "0,");
}
UTIL1_strcatNum16u(cmd, sizeof(cmd), port);
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
res = ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), "OK\r\n", timeoutMs, io);
if (res!=ERR_OK) { /* accept "no change" too */
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSERVER=");
if (startIt) {
UTIL1_strcat(cmd, sizeof(cmd), "1,");
} else {
UTIL1_strcat(cmd, sizeof(cmd), "0,");
}
UTIL1_strcatNum16u(cmd, sizeof(cmd), port);
UTIL1_strcat(cmd, sizeof(cmd), "\r\r\nno change\r\n");
if (UTIL1_strcmp(rxBuf, cmd)==0) {
res = ERR_OK;
}
}
return res;
}
uint8_t ESP_SelectMode(uint8_t mode) {
/* AT+CWMODE=<mode>, where <mode> is 1=Sta, 2=AP or 3=both */
uint8_t txBuf[sizeof("AT+CWMODE=x\r\n")];
uint8_t rxBuf[sizeof("AT+CWMODE=x\r\r\nno change\r\n")];
uint8_t expected[sizeof("AT+CWMODE=x\r\r\nno change\r\n")];
uint8_t res;
if (mode<1 || mode>3) {
return ERR_RANGE; /* only 1, 2 or 3 */
}
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CWMODE=");
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), mode);
UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
UTIL1_strcpy(expected, sizeof(expected), "AT+CWMODE=");
UTIL1_strcatNum16u(expected, sizeof(expected), mode);
UTIL1_strcat(expected, sizeof(expected), "\r\r\n\n");
res = ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, ESP_DEFAULT_TIMEOUT_MS, NULL);
if (res!=ERR_OK) {
/* answer could be as well "AT+CWMODE=x\r\r\nno change\r\n"!! */
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CWMODE=");
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), mode);
UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
UTIL1_strcpy(expected, sizeof(expected), "AT+CWMODE=");
UTIL1_strcatNum16u(expected, sizeof(expected), mode);
UTIL1_strcat(expected, sizeof(expected), "\r\r\nno change\r\n");
if (UTIL1_strcmp(rxBuf, expected)==0) {
res = ERR_OK;
}
}
return res;
}
uint8_t ESP_OpenConnection(int8_t ch_id, bool isTCP, const uint8_t *IPAddrStr, uint16_t port, uint16_t msTimeout, const CLS1_StdIOType *io) {
/* AT+CIPSTART=4,"TCP","184.106.153.149",80 */
uint8_t txBuf[54];
uint8_t rxBuf[72];
uint8_t expected[72];
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CIPSTART=");
if (ch_id>=0) {
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), ch_id);
UTIL1_chcat(txBuf, sizeof(txBuf), ',');
}
if (isTCP) {
UTIL1_strcat(txBuf, sizeof(txBuf), "\"TCP\",\"");
} else {
UTIL1_strcat(txBuf, sizeof(txBuf), "\"UDP\",\"");
}
UTIL1_strcat(txBuf, sizeof(txBuf), IPAddrStr);
UTIL1_strcat(txBuf, sizeof(txBuf), "\",");
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), port);
UTIL1_strcpy(expected, sizeof(expected), txBuf);
UTIL1_strcat(expected, sizeof(expected), "\r\r\n\r\nOK\r\nLinked\r\n");
UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
return ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}
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 TURN_PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[32];
CLS1_SendStatusStr((unsigned char*)"turn", (unsigned char*)"\r\n", io->stdOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), TURN_StepMs);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
CLS1_SendStatusStr((unsigned char*)" step time", buf, io->stdOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), TURN_TimeMs);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
CLS1_SendStatusStr((unsigned char*)" turn time", buf, io->stdOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), TURN_DutyPercent);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
CLS1_SendStatusStr((unsigned char*)" duty", buf, io->stdOut);
}
static void WriteCurrent(MOT_MotorDevice *motor, const CLS1_StdIOType *io) {
unsigned char buf[26];
uint16_t val;
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", current val 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), motor->currentValue);
CLS1_SendStr(buf, io->stdOut);
val = motor->currentValue/(0xFFFF/3300); /* 3300 mV full scale */
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", mV ");
UTIL1_strcatNum16u(buf, sizeof(buf), val);
CLS1_SendStr(buf, io->stdOut);
val = motor->currentValue/(0xFFFF/2000); /* 2000 mA full scale */
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", mA ");
UTIL1_strcatNum16u(buf, sizeof(buf), val);
CLS1_SendStr(buf, io->stdOut);
}
uint8_t ESP_PrepareMsgSend(int8_t ch_id, size_t msgSize, uint16_t msTimeout, const CLS1_StdIOType *io) {
/* AT+CIPSEND=<ch_id>,<size> */
uint8_t cmd[24], rxBuf[48], expected[48];
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSEND="); /* parameters are <ch_id>,<size> */
UTIL1_strcatNum8u(cmd, sizeof(cmd), ch_id);
UTIL1_chcat(cmd, sizeof(cmd), ',');
UTIL1_strcatNum16u(cmd, sizeof(cmd), msgSize);
UTIL1_strcpy(expected, sizeof(expected), cmd); /* we expect the echo of our command */
UTIL1_strcat(expected, sizeof(expected), "\r\r\n> "); /* expect "> " */
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
return ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}
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 void PrintButtonPressed(uint16_t whichButton, bool pressed) {
unsigned char buffer[BUFFER_SIZE];
UTIL1_strcpy(buffer, sizeof(buffer), (uint8_t*)"Push button SW");
UTIL1_strcatNum16u(buffer, sizeof(buffer), whichButton);
if (pressed) {
UTIL1_strcat(buffer, sizeof(buffer), (uint8_t*)" pressed\r\n!");
} else {
UTIL1_strcat(buffer, sizeof(buffer), (uint8_t*)" released\r\n!");
}
#if configUSE_SEGGER_SYSTEM_VIEWER_HOOKS
SEGGER_SYSVIEW_Print((const char*)buffer);
#endif
#if PL_CONFIG_HAS_SHELL_QUEUE
SQUEUE_SendString(buffer);
#endif
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
int i;
uint8_t buf[16], buf2[32];
CLS1_SendStatusStr((uint8_t*)"Floppy", (uint8_t*)"\r\n", io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), FLOPPY_NoteOffset);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"\r\n");
CLS1_SendStatusStr((uint8_t*)" note offset", buf, io->stdOut);
for(i=0;i<FLOPPY_NOF_DRIVES;i++) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)" drive ");
UTIL1_strcatNum16u(buf, sizeof(buf), (uint16_t)i);
UTIL1_chcat(buf, sizeof(buf), ':');
buf2[0] = '\0';
if (FLOPPY_Drives[i].forward) {
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"fw ");
} else {
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"bw ");
}
UTIL1_strcatNum16s(buf2, sizeof(buf2), FLOPPY_Drives[i].pos);
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"\r\n");
CLS1_SendStatusStr(buf, buf2, io->stdOut);
}
return ERR_OK;
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
#if PID_DEBUG
unsigned char buf[16];
static uint8_t cnt = 0;
#endif
uint8_t errorPercent;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
errorPercent = errorWithinPercent(currLine-setLine);
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed;
}
} else if (errorPercent <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */
}
} else if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
}
} else {
/* line is far to the left or right: use backward motor motion */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
if (pid<0) { /* turn right */
speedR = -speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = -speed-pid; /* decrease speed */
}
speedL = Limit(speedL, -speed, speed);
speedR = Limit(speedR, -speed, speed);
directionL = AbsSpeed(&speedL);
directionR = AbsSpeed(&speedR);
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
{
cnt++;
if (cnt>10) { /* limit number of messages to the console */
CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
cnt = 0;
CLS1_SendStr((unsigned char*)"line:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" sum:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" left:", ioOut);
CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" right:", ioOut);
CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)"\r\n", ioOut);
}
}
#endif
}
