static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" state", REF_GetStateString(), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" raw val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" min val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.minVal[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" max val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.maxVal[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calib val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
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);
}
void loop(void) {
Serial_println("Sending to nrf24_reliable_datagram_server");
// Send a message to manager_server
if (manager.sendtoWait(data, sizeof(data), SERVER_ADDRESS))
{
// Now wait for a reply from the server
uint8_t len = sizeof(buf);
uint8_t from;
if (manager.recvfromAckTimeout(buf, &len, 2000, &from))
{
uint8_t buf[16];
Serial_print("got reply from : 0x");
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), from);
Serial_print((const char*)buf);
Serial_print(": ");
Serial_println((char*)buf);
} else {
Serial_println("No reply, is nrf24_reliable_datagram_server running?");
}
} else {
Serial_println("sendtoWait failed");
}
delay(500);
}
static uint8_t HandleDataRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_HAS_SHELL
uint8_t buf[16];
CLS1_ConstStdIOTypePtr io = CLS1_GetStdio();
#endif
uint8_t val;
(void)size;
(void)packet;
switch(type) {
case RAPP_MSG_TYPE_DATA: /* <type><size><data */
*handled = TRUE;
val = *data; /* get data value */
#if PL_HAS_SHELL
CLS1_SendStr((unsigned char*)"Data: ", io->stdOut);
CLS1_SendNum8u(val, io->stdOut);
CLS1_SendStr((unsigned char*)" from addr 0x", io->stdOut);
buf[0] = '\0';
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), srcAddr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), srcAddr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStr(buf, io->stdOut);
#endif
return ERR_OK;
default:
break;
} /* switch */
return ERR_OK;
}
static uint8_t HandleDataRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_HAS_SHELL
uint8_t buf[32];
CLS1_ConstStdIOTypePtr io = CLS1_GetStdio();
#endif
uint8_t val;
(void)size;
(void)packet;
switch(type) {
case RAPP_MSG_TYPE_DATA: /* generic data message */
*handled = TRUE;
val = *data; /* get data value */
#if PL_HAS_DRIVE
DRV_EnableDisable(TRUE);
#endif
#if PL_HAS_SHELL
SHELL_SendString((unsigned char*)"Data: ");
SHELL_SendString(data);
SHELL_SendString((unsigned char*)" from addr 0x");
buf[0] = '\0';
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), srcAddr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), srcAddr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
#endif /* PL_HAS_SHELL */
return ERR_OK;
break;
default:
break;
} /* switch */
return ERR_OK;
}
static uint8_t Tune(const CLS1_StdIOType *io, uint8_t channel, MOT_MotorDevice *motorHandle) {
#define TUNE_MOTOR_PERCENT 20
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[48];
uint8_t res;
//#if PL_HAS_DRIVE
// DRV_SetMode(DRV_MODE_NONE); /* turn off drive mode */
//#endif
MOT_SetSpeedPercent(motorHandle, TUNE_MOTOR_PERCENT);
CLS1_SendStr((uint8_t*)"Tuning channel...\r\n", io->stdOut);
res = ERR_FAILED;
for(i=0,dac=0;dac<=MCP4728_MAX_DAC_VAL;i++) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_chcat(buf, sizeof(buf), (uint8_t)('A'+channel)); /* 0:A, 1:B, 2:C, 3:D */
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
UTIL1_chcat(buf, sizeof(buf), ' ');
CLS1_SendStr(buf, io->stdOut);
if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) { /* writes single channel DAC value, not updating EEPROM */
CLS1_SendStr((uint8_t*)"ERROR writing DAC channel!\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_WaitOSms(100); /* wait some time to allow DAC and OP-Amp change */
if (Measure(channel, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
CLS1_SendStr(buf, io->stdOut);
if (timing.highPercent==50 || timing.lowPercent==50) {
CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
if (MCP4728_WriteDACandEE(channel, dac)!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EEPROM\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
CLS1_SendStr((uint8_t*)"...done!\r\n", io->stdOut);
res = ERR_OK;
break; /* go to next channel */
}
dac += 0x1; /* smaller increase */
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
dac += 0x10; /* larger increase */
}
} /* for finding DAC value */
MOT_SetSpeedPercent(motorHandle, 0); /* turn off again */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
}
CLS1_SendStr((uint8_t*)"Tuning finished!\r\n", io->stdOut);
return res;
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[24];
uint16_t val;
CLS1_SendStatusStr((unsigned char*)"VS1053", (unsigned char*)"\r\n", io->stdOut);
if (VS_ReadRegister(VS_MODE, &val)==ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), "ERROR\r\n");
}
CLS1_SendStatusStr((unsigned char*)" MODE", buf, io->stdOut);
if (VS_ReadRegister(VS_STATUS, &val)==ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), "ERROR\r\n");
}
CLS1_SendStatusStr((unsigned char*)" STATUS", buf, io->stdOut);
if (VS_ReadRegister(VS_CLOCKF, &val)==ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), "ERROR\r\n");
}
CLS1_SendStatusStr((unsigned char*)" CLOCKF", buf, io->stdOut);
if (VS_ReadRegister(VS_VOL, &val)==ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), "ERROR\r\n");
}
CLS1_SendStatusStr((unsigned char*)" VOLUME", buf, io->stdOut);
return ERR_OK;
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[15];
int i;
CLS1_SendStatusStr((unsigned char*)"Reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" led", ledON?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrated", isCalibrated?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" raw", (unsigned char*)"", io->stdOut);
for (i=0;i<NOF_SENSORS;i++) {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" white", (unsigned char*)"", io->stdOut);
for (i=0;i<NOF_SENSORS;i++) {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorWhite[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" black", (unsigned char*)"", io->stdOut);
for (i=0;i<NOF_SENSORS;i++) {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorBlack[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrated", (unsigned char*)"", io->stdOut);
for (i=0;i<NOF_SENSORS;i++) {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" line", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16s(buf, sizeof(buf), refLine);
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
static void StatusPrintXY(CLS1_ConstStdIOType *io) {
uint16_t x, y;
int8_t x8, y8;
uint8_t buf[64];
if (APP_GetXY(&x, &y, &x8, &y8)==ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"X: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), x);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"(");
UTIL1_strcatNum8s(buf, sizeof(buf), x8);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)") Y: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"(");
UTIL1_strcatNum8s(buf, sizeof(buf), y8);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"GetXY() failed!\r\n");
}
CLS1_SendStatusStr((unsigned char*)" analog", buf, io->stdOut);
}
void REF_DumpHistory(void) {
int i;
unsigned char buf[16];
CLS1_SendStr((unsigned char*)"history: #", CLS1_GetStdio()->stdOut);
CLS1_SendNum16u(REF_nofHistory, CLS1_GetStdio()->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
CLS1_SendStr((unsigned char*)" 0x", CLS1_GetStdio()->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorHistory[i]);
CLS1_SendStr(buf, CLS1_GetStdio()->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->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);
}
static uint8_t HandleDataRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_HAS_SHELL
uint8_t buf[32];
CLS1_ConstStdIOTypePtr io = CLS1_GetStdio();
#endif
uint8_t val;
protocol42 rxdata;
(void)size;
(void)packet;
switch(type) {
case RAPP_MSG_TYPE_DATA: /* generic data message */
*handled = TRUE;
val = *data; /* get data value */
#if PL_HAS_SHELL //TODO SEND AN SHELQUEUE CBR
CLS1_SendStr((unsigned char*)"Data: ", io->stdOut);
CLS1_SendNum8u(val, io->stdOut);
CLS1_SendStr((unsigned char*)" from addr 0x", io->stdOut);
buf[0] = '\0';
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), srcAddr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), srcAddr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStr(buf, io->stdOut);
#endif /* PL_HAS_SHELL */
return ERR_OK;
case RAPP_MSG_TYPE_PROTOCOL42:
rxdata.target = (int8_t)*data;
rxdata.type = (int8_t)*(data+1);
rxdata.data = (*(data+2));
reciveData42(rxdata);
default: /*! \todo Handle your own messages here */
break;
} /* switch */
return ERR_OK;
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[32];
CLS1_SendStatusStr((unsigned char*)"app", (unsigned char*)"\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), APP_dstAddr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), APP_dstAddr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" dest addr", buf, io->stdOut);
return ERR_OK;
}
static void RemoteTask (void *pvParameters) {
(void)pvParameters;
#if PL_CONFIG_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
for(;;) {
if (REMOTE_isOn) {
#if PL_CONFIG_HAS_JOYSTICK
if (REMOTE_useJoystick) {
uint8_t buf[2];
int16_t x, y;
int8_t x8, y8;
/* send periodically messages */
APP_GetXY(&x, &y, &x8, &y8);
buf[0] = x8;
buf[1] = y8;
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
FRTOS1_vTaskDelay(200/portTICK_PERIOD_MS);
} else {
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
}
} /* for */
}
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);
}
void loop(void) {
if (manager.available()) {
// Wait for a message addressed to us from the client
uint8_t len = sizeof(buf);
uint8_t from;
if (manager.recvfromAck(buf, &len, &from))
{
uint8_t buf[16];
Serial_print("got request from : 0x");
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), from);
Serial_print((const char*)buf);
Serial_print(": ");
Serial_println((char*)buf);
// Send a reply back to the originator client
if (!manager.sendtoWait(data, sizeof(data), from))
Serial_println("sendtoWait failed");
}
}
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" state", REF_GetStateString(), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min noise", buf, io->stdOut);
CLS1_SendStatusStr((unsigned char*)" raw val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" min val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.minVal[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" max val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.maxVal[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calib val", (unsigned char*)"", io->stdOut);
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" line val", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#if PL_CONFIG_HAS_LINE_FOLLOW
CLS1_SendStatusStr((unsigned char*)" line kind", REF_LineKindStr(refLineKind), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#endif
return ERR_OK;
}
static portTASK_FUNCTION(RemoteTask, pvParameters) {
#if PL_HAS_WATCHDOG
int i;
#endif
(void)pvParameters;
#if PL_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
for(;;) {
if (REMOTE_isOn) {
#if PL_HAS_ACCEL
if (REMOTE_useAccelerometer) {
#if PL_HAS_KEYS
uint8_t buf[7];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[6];
#endif /* PL_HAS_KEYS */
int16_t x, y, z;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
ACCEL_GetValues(&x, &y, &z);
buf[0] = (uint8_t)(x&0xFF);
buf[1] = (uint8_t)(x>>8);
buf[2] = (uint8_t)(y&0xFF);
buf[3] = (uint8_t)(y>>8);
buf[4] = (uint8_t)(z&0xFF);
buf[5] = (uint8_t)(z>>8);
#if PL_HAS_KEYS
buf[6] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), x);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), y);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" z: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), z);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_ACCEL, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_JOYSTICK
if (REMOTE_useJoystick) {
#if PL_HAS_KEYS
uint8_t buf[3];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[2];
#endif /* PL_HAS_KEYS */
int16_t x, y;
int8_t x8, y8;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
APP_GetXY(&x, &y, &x8, &y8);
buf[0] = x8;
buf[1] = y8;
#if PL_HAS_KEYS
buf[2] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_WATCHDOG
for(i=0; i<2; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(200/portTICK_RATE_MS);
#endif
} else {
#if PL_HAS_WATCHDOG
for(i=0; i<10; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
#endif
}
} /* for */
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" IR led on", ledON?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrating", doMinMaxCalibration?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrated", isCalibrated?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min noise", buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_LINE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min line", buf, io->stdOut);
CLS1_SendStatusStr((unsigned char*)" raw val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" min val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMin[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" max val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMax[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calib val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" line val", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#if PL_APP_LINE_MAZE
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStatusStr((unsigned char*)" history", (unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorHistory[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#endif
CLS1_SendStatusStr((unsigned char*)" line kind", REF_LineKindStr(refLineKind), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
byte REF_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "ref help")==0) {
*handled = TRUE;
return PrintHelp(io);
} else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "ref status")==0)) {
*handled = TRUE;
return PrintStatus(io);
} else if (UTIL1_strcmp((char*)cmd, "ref calib on")==0) {
APP_StateStartCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref calib off")==0) {
APP_StateStopCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led on")==0) {
ledON = TRUE;
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led off")==0) {
ledON = FALSE;
*handled = TRUE;
return ERR_OK;
}
return ERR_OK;
}
uint16_t REF_GetLineValue(bool *onLine) {
*onLine = refCenterLineVal>0 && refCenterLineVal<REF_MAX_LINE_VALUE;
return refCenterLineVal;
}
static portTASK_FUNCTION(ReflTask, pvParameters) {
(void)pvParameters; /* not used */
for(;;) {
if (doMinMaxCalibration) {
REF_CalibrateMinMax(SensorMin, SensorMax, SensorRaw);
#if PL_HAS_BUZZER
BUZ_Beep(300, 50);
#endif
} else {
REF_Measure();
}
FRTOS1_vTaskDelay(10/portTICK_RATE_MS);
}
}
void REF_Init(void) {
refLineKind = REF_LINE_NONE;
refCenterLineVal = 0;
mutexHandle = FRTOS1_xSemaphoreCreateMutex();
if (mutexHandle==NULL) {
for(;;);
}
timerHandle = RefCnt_Init(NULL);
REF_InitSensorValues();
if (FRTOS1_xTaskCreate(ReflTask, (signed portCHAR *)"Refl", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+1+2, NULL) != pdPASS) {
for(;;){} /* error */
}
}
void RAPP_SniffPacket(RPHY_PacketDesc *packet, bool isTx) {
uint8_t buf[32];
const CLS1_StdIOType *io;
int i;
uint8_t dataSize;
RNWK_ShortAddrType addr;
io = CLS1_GetStdio();
if (isTx) {
CLS1_SendStr((unsigned char*)"Packet Tx ", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)"Packet Rx ", io->stdOut);
}
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"flags: ");
UTIL1_strcatNum16s(buf, sizeof(buf), packet->flags);
CLS1_SendStr(buf, io->stdOut);
if (packet->flags!=RPHY_PACKET_FLAGS_NONE) {
CLS1_SendStr((unsigned char*)"(", io->stdOut);
if (packet->flags&RPHY_PACKET_FLAGS_IS_ACK) {
CLS1_SendStr((unsigned char*)"IS_ACK,", io->stdOut);
}
if (packet->flags&RPHY_PACKET_FLAGS_REQ_ACK) {
CLS1_SendStr((unsigned char*)"REQ_ACK", io->stdOut);
}
CLS1_SendStr((unsigned char*)")", io->stdOut);
}
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" size: ");
UTIL1_strcatNum16s(buf, sizeof(buf), packet->phySize);
CLS1_SendStr(buf, io->stdOut);
/* PHY */
CLS1_SendStr((unsigned char*)" PHY data: ", io->stdOut);
dataSize = RPHY_BUF_SIZE(packet->phyData);
for(i=0; i<dataSize+RPHY_HEADER_SIZE;i++) {
buf[0] = '\0';
UTIL1_strcatNum8Hex(buf, sizeof(buf), packet->phyData[i]);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ");
CLS1_SendStr(buf, io->stdOut);
}
/* MAC */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" MAC size:");
UTIL1_strcatNum8u(buf, sizeof(buf), dataSize);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" type:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_TYPE(packet->phyData));
CLS1_SendStr(buf, io->stdOut);
RMAC_DecodeType(buf, sizeof(buf), packet);
CLS1_SendStr(buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" s#:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_SEQN(packet->phyData));
CLS1_SendStr(buf, io->stdOut);
/* NWK */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" NWK src:");
addr = RNWK_BUF_GET_SRC_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" dst:");
addr = RNWK_BUF_GET_DST_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
CLS1_SendStr(buf, io->stdOut);
/* APP */
if (dataSize>RMAC_HEADER_SIZE+RNWK_HEADER_SIZE) { /* there is application data */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" APP type:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_TYPE(packet->phyData));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" size:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_SIZE(packet->phyData));
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 */
}
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
}
static uint8_t Tune(const CLS1_StdIOType *io) {
uint8_t channel;
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[32];
uint8_t res;
uint16_t dacArr[4];
uint8_t pd[4];
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 10);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 10);
for(i=0;i<4;i++) { /* init */
dacArr[i] = 0;
pd[i] = 0;
}
CLS1_SendStr((uint8_t*)"Tuning....\r\n", io->stdOut);
res = ERR_OK;
for(channel=0;channel<NOF_SIGNALS;channel++) {
for(i=0,dac=0;i<80 && dac<MPC4728_MAX_DAC_VAL;i++,dac += 0x10) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_strcatNum8u(buf, sizeof(buf), channel);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
UTIL1_chcat(buf, sizeof(buf), ' ');
CLS1_SendStr(buf, io->stdOut);
dacArr[channel] = dac;
if (MPC4728_FastWriteDAC(dacArr, sizeof(dacArr), pd, sizeof(pd))!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(50); /* wait some time to allow change */
if (Measure(channel, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
CLS1_SendStr(buf, io->stdOut);
if (timing.highPercent==50 || timing.lowPercent==50) {
CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
break;
}
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
}
} /* for */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
break;
}
} /* for */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
for (channel=0;channel<NOF_SIGNALS;channel++) {
if (MPC4728_SingleWriteDACandEE(channel, dacArr[channel])!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(500); /* give EEPROM time to write data */
}
CLS1_SendStr((uint8_t*)"finished!\r\n", io->stdOut);
return res;
}
