static void PrintTouchText(unsigned char line) {
unsigned char buf[32];
if (evnt==TCHS1_TOUCHSCREEN_EVENT_TOUCH) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"Touch: x:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), xPos, '0', 3);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" y:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), yPos, '0', 3);
} else if (evnt==TCHS1_TOUCHSCREEN_EVENT_MOVE) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"Move: x:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), xPos, '0', 3);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" y:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), yPos, '0', 3);
} else if (evnt==TCHS1_TOUCHSCREEN_EVENT_RELEASE) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"Release: x:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), xPos, '0', 3);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" y:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), yPos, '0', 3);
} else {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"Touch me!");
}
PrintLCDText(line, buf);
}
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);
}
void RADIO_SendString(const char *data) {
if (!RADIO_isOn) {
return;
}
while (RADIO_AppStatus != RADIO_READY_FOR_TX_RX_DATA) { /* we are not ready yet! */
RADIO_HandleState(); /* advance state machine */
}
UTIL1_strcpy((char*)RADIO_TxDataBuffer, sizeof(RADIO_TxDataBuffer), RADIO_PREFIX_STR);
UTIL1_strcat((char*)RADIO_TxDataBuffer, sizeof(RADIO_TxDataBuffer), data);
RADIO_TxPacket.pu8Data = &RADIO_TxDataBuffer[0]; /* Load the address of our txbuffer into tx structure.*/
RADIO_TxPacket.u8DataLength = (byte)(UTIL1_strlen((char*)RADIO_TxDataBuffer)+1); /* Set the data length of the packet */
RADIO_AppStatus = RADIO_TRANSMIT_DATA;
RADIO_HandleState(); /* advance state machine */
}
/*!
\brief This routine is called as callback by the radio driver on reception of a data packet
\param msg Pointer to the message we received.
*/
static void RADIO_HandleMessage(uint8_t *msg) {
char buf[32];
uint8_t i, size;
if (RADIO_isSniffing && *msg==RADIO_QUEUE_MSG_SNIFF) {
msg++;
size = *msg++;
UTIL1_strcpy(buf, sizeof(buf), "\r\nch #:"); /* use new line at the beginning, as the hex dump at the end might be fill up buffer completely */
UTIL1_strcatNum16s(buf, sizeof(buf), RADIO_Channel);
UTIL1_strcat(buf, sizeof(buf), " size:");
UTIL1_strcatNum16s(buf, sizeof(buf), size);
UTIL1_strcat(buf, sizeof(buf), " ASCII: ");
SHELL_SendMessage(buf);
buf[0] = '\0';
/* write as string */
for(i=0;i<size && i<sizeof(buf);i++) {
UTIL1_chcat(buf, sizeof(buf), msg[i]);
}
SHELL_SendMessage(buf);
/* write as hex */
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), " hex: ");
for(i=0; i<size;i++) {
UTIL1_strcatNum8Hex(buf, sizeof(buf), msg[i]);
UTIL1_strcat(buf, sizeof(buf), " ");
}
SHELL_SendMessage(buf);
SHELL_SendMessage("\r\n");
#if PL_HAS_REMOTE && PL_HAS_MOTOR
/*! \todo Implement handling for your remote control */
} else if (*msg==RADIO_QUEUE_MSG_ACCEL) {
msg++;
size = *msg++;
REMOTE_ParseMsg((const unsigned char*)msg+sizeof(RADIO_PREFIX_STR)-1, size-sizeof(RADIO_PREFIX_STR)-1);
#endif
}
}
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) {
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;
}
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 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
}
uint8_t ESP_SetNumberOfConnections(uint8_t nof, const CLS1_StdIOType *io, uint16_t timeoutMs) {
/* AT+CIPMUX=<nof>, 0: single connection, 1: multiple connections */
uint8_t res;
uint8_t cmd[sizeof("AT+CIPMUX=12\r\n")];
uint8_t rxBuf[sizeof("AT+CIPMUX=12\r\n\r\nOK\r\n")+10];
if (nof>1) { /* only 0 and 1 allowed */
if (io!=NULL) {
CLS1_SendStr("Wrong number of connection parameter!\r\n", io->stdErr);
}
return ERR_FAILED;
}
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPMUX=");
UTIL1_strcatNum8u(cmd, sizeof(cmd), nof);
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
res = ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), "OK\r\n", timeoutMs, io);
return res;
}
static uint8_t JoinAccessPoint(const uint8_t *ssid, const uint8_t *pwd, CLS1_ConstStdIOType *io) {
/* AT+CWJAP="<ssid>","<pwd>" */
uint8_t txBuf[48];
uint8_t rxBuf[64];
uint8_t expected[48];
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CWJAP=\"");
UTIL1_strcat(txBuf, sizeof(txBuf), ssid);
UTIL1_strcat(txBuf, sizeof(txBuf), "\",\"");
UTIL1_strcat(txBuf, sizeof(txBuf), pwd);
UTIL1_strcat(txBuf, sizeof(txBuf), "\"\r\n");
UTIL1_strcpy(expected, sizeof(expected), "AT+CWJAP=\"");
UTIL1_strcat(expected, sizeof(expected), ssid);
UTIL1_strcat(expected, sizeof(expected), "\",\"");
UTIL1_strcat(expected, sizeof(expected), pwd);
UTIL1_strcat(expected, sizeof(expected), "\"\r\r\n\r\nOK\r\n");
return ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, ESP_DEFAULT_TIMEOUT_MS, io);
}
static void CDC_Run(void) {
int i, cnt = 0;
uint32_t val = 0;
unsigned char buf[16];
(void)CDC1_SendString((unsigned char*)"Hello world from the KL25Z with USB CDC\r\n");
for(;;) {
while(CDC1_App_Task(cdc_buffer, sizeof(cdc_buffer))==ERR_BUSOFF) {
/* device not enumerated */
LED1_Neg(); LED2_Off();
WAIT1_Waitms(10);
cnt++;
if (cnt==100) {
LED3_Neg();
cnt = 0;
}
}
LED1_Off(); LED2_Neg(); LED3_Off();
if (CDC1_GetCharsInRxBuf()!=0) {
i = 0;
while( i<sizeof(in_buffer)-1
&& CDC1_GetChar(&in_buffer[i])==ERR_OK
)
{
i++;
}
in_buffer[i] = '\0';
(void)CDC1_SendString((unsigned char*)"echo: ");
(void)CDC1_SendString(in_buffer);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"val: ");
UTIL1_strcatNum32u(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
(void)CDC1_SendString(buf);
val++;
} else {
WAIT1_Waitms(10);
cnt++;
if ((cnt%1024)==0) { /* from time to time, write some text */
(void)CDC1_SendString((unsigned char*)"Type some text and it will echo.\r\n");
}
}
}
}
static portTASK_FUNCTION(TraceTask, pvParameters) {
static unsigned char buf[128]; /* global buffer, to reduce stack size. We want to send things in one piece */
(void)pvParameters;
for(;;) {
if (traceChannel==TRACE_TO_NONE) {
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
} else if (traceChannel==TRACE_TO_SHELL) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
if (traceAccel) {
int16_t x, y, z;
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);
CLS1_SendStr(&buf[0], CLS1_GetStdio()->stdOut);
}
if (traceMagnetometer) {
int16_t x, y, z;
MAG_GetValues(&x, &y, &z);
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" MX:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; MY:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; MZ:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
CLS1_SendStr(&buf[0], CLS1_GetStdio()->stdOut);
}
if (traceMagnetometer || traceAccel) {
CLS1_SendStr((unsigned char*)"\r\n", CLS1_GetStdio()->stdOut);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
} else {
FRTOS1_vTaskDelay(25/portTICK_RATE_MS);
}
}
}
}
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);
}
static void CDC_Run(void) {
unsigned char buf[32];
for(;;) {
while(CDC1_App_Task(cdc_buffer, sizeof(cdc_buffer))==ERR_BUSOFF) {
/* device not enumerated */
LEDR_Neg(); LEDG_Off();
WAIT1_Waitms(10);
}
LEDR_Off(); LEDG_Neg();
if (CDC1_GetCharsInRxBuf()!=0) {
CDC1_ClearRxBuffer(); /* discard incoming buffer */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"time on hours: ");
UTIL1_strcatNum32u(buf, sizeof(buf), Time_On_Hours);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
(void)CDC1_SendString(buf);
Time_On_Hours++;
} else {
WAIT1_Waitms(10);
}
}
}
static void LogToFile(int16_t x, int16_t y, int16_t z) {
uint8_t write_buf[48];
UINT bw;
TIMEREC time;
/* open file */
if (FAT1_open(&fp, "./log.txt", FA_OPEN_ALWAYS|FA_WRITE)!=FR_OK) {
Err();
}
/* move to the end of the file */
if (FAT1_lseek(&fp, fp.fsize) != FR_OK || fp.fptr != fp.fsize) {
Err();
}
/* get time */
if (TmDt1_GetTime(&time)!=ERR_OK) {
Err();
}
/* write data */
write_buf[0] = '\0';
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Hour);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Min);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Sec);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), x);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), y);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), z);
UTIL1_strcat(write_buf, sizeof(write_buf), (unsigned char*)"\r\n");
if (FAT1_write(&fp, write_buf, UTIL1_strlen((char*)write_buf), &bw)!=FR_OK) {
(void)FAT1_close(&fp);
Err();
}
/* closing file */
(void)FAT1_close(&fp);
}
uint8_t ESP_SendStr(const uint8_t *str, CLS1_ConstStdIOType *io) {
uint8_t buf[32];
uint8_t rxBuf[48];
uint8_t res;
uint16_t timeoutMs;
#define RX_TIMEOUT_MS 3000
AS2_TComData ch;
UTIL1_strcpy(buf, sizeof(buf), str);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
res = ESP_SendATCommand(buf, rxBuf, sizeof(rxBuf), NULL, ESP_DEFAULT_TIMEOUT_MS, io);
timeoutMs = 0;
while(timeoutMs<RX_TIMEOUT_MS) {
WAIT1_WaitOSms(100);
timeoutMs += 100;
while (AS2_GetCharsInRxBuf()>0) {
(void)AS2_RecvChar(&ch);
CLS1_SendChar(ch);
}
}
return ERR_OK;
}
void APP_Run(void) {
int i;
uint32_t val = 0;
unsigned char buf[16];
Buzzer_SetRatio16(0xFFFF/2); /* 50% duty ratio */
for(;;) {
while(CDC1_App_Task(cdc_buffer, sizeof(cdc_buffer))==ERR_BUSOFF) {
/* device not enumerated */
LED1_Neg(); LED2_Off();
LED3_Neg(); LED4_Off();
WAIT1_Waitms(10);
}
LED1_Off(); LED2_Neg();
LED3_Off(); LED4_Neg();
if (CDC1_GetCharsInRxBuf()!=0) {
i = 0;
while( i<sizeof(in_buffer)-1
&& CDC1_GetChar(&in_buffer[i])==ERR_OK
)
{
i++;
}
in_buffer[i] = '\0';
(void)CDC1_SendString((unsigned char*)"echo: ");
(void)CDC1_SendString(in_buffer);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"val: ");
UTIL1_strcatNum32u(buf, sizeof(buf), val);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
(void)CDC1_SendString(buf);
val++;
} else {
WAIT1_Waitms(10);
}
}
}
static void Log(unsigned char *msg, const CLS1_StdIOType *io) {
RTC1_TTIME currTime;
RTC1_TDATE currDate;
unsigned char buf[32];
if (RTC1_GetRTCDate(&currDate)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"*** Failed to get RTC date\r\n", io->stdErr);
} else {
buf[0] = '\0';
StrCatDate(buf, sizeof(buf), currDate.day, currDate.month, currDate.year+2000);
CLS1_SendStr(buf, io->stdOut);
}
if (RTC1_GetRTCTime(&currTime)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"*** Failed to get RTC time\r\n", io->stdErr);
} else {
buf[0] = '\0';
UTIL1_chcat(buf, sizeof(buf), ' ');
StrCatTime(buf, sizeof(buf), currTime.hour, currTime.min, currTime.sec);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)": ");
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr(msg, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", 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 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);
if(x8 > 100)
buf[0] = 1;
else if(x8 < -100)
buf[0] = 2;
else
buf[0] = 0;
//buf[0] = x8;
if(normalSpeedPressed) {
if(fastSpeedPressed) {
buf[1] = 2; // fast speed
} else {
buf[1] = 1; // normal speed
}
} else if (retourSpeedPressed) {
buf[1] = 3; // retour
} else {
buf[1] = 0; // stand
}
if(startLineFollow == 1) {
buf[1] = 4; // left handed
startLineFollow = 0;
}
if(startLineFollow == 2) {
buf[1] = 5; // right handed
startLineFollow = 0;
}
//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*)" 0/R/L: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), buf[0]);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" 0/N/F/R: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), buf[1]);
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(20/portTICK_PERIOD_MS);
} else {
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
}
} /* for */
}
static void BleUartTask(void *pvParameters) {
uint8_t buf[MAX_TX_MSG_SIZE];
uint8_t txBuf[MAX_TX_MSG_SIZE+sizeof("[Tx] ")+sizeof("AT+BLEUARTTX=\n")];
uint8_t res;
CLS1_ConstStdIOType *io = CLS1_GetStdio();
int i;
bool prevIsEnabled = FALSE;
BLE_Init(); /* initialize BLE module, has to be done when interrupts are enabled */
FRTOS1_vTaskDelay(pdMS_TO_TICKS(2000)); /* wait so other tasks have time to write to the Shell */
if (res!=ERR_OK) {
CLS1_SendStr("Failed changing LED activity\r\n", io->stdErr);
}
CLS1_SendStr("******************************************\r\n", io->stdOut);
CLS1_SendStr("* Adafruit BLE UART CMD Mode Application *\r\n", io->stdOut);
CLS1_SendStr("******************************************\r\n", io->stdOut);
for(;;) {
if (!prevIsEnabled && isEnabled) { /* enabled now */
prevIsEnabled = TRUE;
BLE_Echo(FALSE); /* Disable command echo from Bluefruit */
CLS1_SendStr("Changing LED activity to MODE\r\n", io->stdOut);
res = BLE_SendATCommandExpectedResponse("AT+HWMODELED=1\n", buf, sizeof(buf), "OK\r\n"); /* NOTE: "=MODE" failed! */
if (res!=ERR_OK) {
CLS1_SendStr("Failed setting LED mode.\r\n", io->stdErr);
}
CLS1_SendStr("BLE UART enabled.\r\n", io->stdOut);
} else if (prevIsEnabled && !isEnabled) { /* disabled now */
prevIsEnabled = FALSE;
CLS1_SendStr("BLE UART disabled.\r\n", io->stdOut);
}
if (isEnabled) {
while(isEnabled && !(isConnected=BLE_IsConnected())) { /* wait until connected */
CLS1_SendStr("Waiting for BLE UART connection...\r\n", io->stdOut);
for(i=0;i<5 && isEnabled;i++) {
FRTOS1_vTaskDelay(pdMS_TO_TICKS(1000));
LED1_Neg();
}
}
if (isConnected) {
CLS1_SendStr("Connected!\r\n", io->stdOut);
}
while(isEnabled) { /* will break */
isConnected=BLE_IsConnected();
if (!isConnected) {
CLS1_SendStr("Disconnected!\r\n", io->stdOut);
break; /* get out of loop */
}
if (txBuffer[0]!='\0') { /* have something to tx */
/* copy buffer */
taskENTER_CRITICAL();
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+BLEUARTTX=");
UTIL1_strcat(txBuf, sizeof(txBuf), "[Tx] ");
UTIL1_strcat(txBuf, sizeof(txBuf), txBuffer);
txBuffer[0] = '\0';
taskEXIT_CRITICAL();
/* send tx string */
res = BLE_SendATCommandExpectedResponse(txBuf, buf, sizeof(buf), "OK\r\n");
if (res!=ERR_OK) {
CLS1_SendStr("Failed to Tx string\r\n", io->stdErr);
}
}
/* check Rx */
res = BLE_SendATCommandExpectedResponse("AT+BLEUARTRX\n", buf, sizeof(buf), "OK\r\n");
if (res==ERR_OK) {
if (UTIL1_strncmp(buf, "OK\r\n", sizeof("OK\r\n")-1)==0) {
/* only OK as response: no data */
} else {
/* print response */
UTIL1_strCutTail(buf, "OK\r\n"); /* cut off the OK part */
CLS1_SendStr("[Rx] ", io->stdOut);
CLS1_SendStr(buf, io->stdOut);
}
}
FRTOS1_vTaskDelay(pdMS_TO_TICKS(50));
LED1_Neg();
} /* while */
} else {
FRTOS1_vTaskDelay(pdMS_TO_TICKS(500));
LED1_Neg();
}
//FRTOS1_vTaskDelay(pdMS_TO_TICKS(1000));
}
}
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;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-15 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button - disable remote, drive mode none */
DRV_SetMode(DRV_MODE_NONE);
REMOTE_SetOnOff(FALSE);
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
REMOTE_SetOnOff(TRUE);
#if PL_CONFIG_HAS_DRIVE
DRV_SetMode(DRV_MODE_SPEED);
#endif
SHELL_SendString("Remote ON\r\n");
}
#if PL_CONFIG_HAS_LINE_MAZE
else if (val=='A') { /* green 'A' button */
SHELL_SendString("Button A pressed\r\n");
// Start Maze solving
if(!LF_IsFollowing()){
LF_StartFollowing();
}
} else if (val=='K') { /* green A button longpress -> 'K' */
SHELL_SendString("Clear Maze\r\n");
// Clear old maze solution, ready for restart
MAZE_ClearSolution();
} else if (val =='B'){ /* yellow 'B' button */
SHELL_SendString("Right hand rule!\r\n");
LF_SetRule(FALSE);
} else if (val=='E') { /* button 'E' pressed */
SHELL_SendString("Stop Following! \r\n");
if(LF_IsFollowing()){
LF_StopFollowing();
}
} else if (val=='D') { /* blue 'D' button */
SHELL_SendString("Left hand rule!\r\n");
LF_SetRule(TRUE);
}
#endif
else if (val=='C') { /* red 'C' button */
NITRO = TRUE;
SHELL_SendString("Nitrooooooo!!!\r\n");
BUZ_Beep(1000,1000);
} else if (val=='J') { /* button 'C' released */
NITRO = FALSE;
SHELL_SendString("Stop Nitro \r\n");
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
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 void RemoteTask(void *pvParameters) {
(void) pvParameters;
#if PL_HAS_JOYSTICK
(void) APP_GetXY_Joystick(&midPointX, &midPointY, NULL, NULL);
#endif
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
for (;;) {
if (REMOTE_isOn) {
#if PL_HAS_ACCEL
if(REMOTE_useAccel){
uint8_t buf[6];
int16_t x, y, z;
/*send periodically messages*/
ACCEL_getXYZmg(&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 (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*) "\r\n y: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), y);
UTIL1_strcat(txtBuf, sizeof(txtBuf),
(unsigned char*) "\r\n z: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), z);
UTIL1_strcat(txtBuf, sizeof(txtBuf),
(unsigned char*) "\r\n 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) {
uint8_t buf[2];
int16_t x, y;
int8_t x8, y8;
/* send periodically messages */
APP_GetXY_Joystick(&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_RATE_MS);
} else {
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
}
} /* for */
void DHT11() {
byte dht11_dat[5];
byte dht11_in;
byte i;
// output
DHT_SetDir(1);
// pull-down i/o pin for 18ms
DHT_ClrVal();
WAIT1_Waitms(19);
DHT_SetVal();
//pull-up i/o pin for 30ms
WAIT1_Waitus(30);
//input
DHT_SetDir(0);
WAIT1_Waitus(30);
while(!dht11_in)
{
dht11_in = DHT_GetVal();
}
// if (dht11_in) {
// printf("dht11 start condition 1 not met\r\n");
// return;
// }
WAIT1_Waitus(80);//delayMicroseconds(80);
// dht11_in = DHT_GetVal();//dht11_in = PINC & _BV(DHT11_PIN);
// if (!dht11_in) {
// printf("dht11 start condition 2 not met\r\n");
// return;
// }
// WAIT1_Waitus(80);//delayMicroseconds(80);
// now ready for data reception
for (i = 0; i < 5; i++)
dht11_dat[i] = read_dht11_dat();
DHT_SetDir(1);
DHT_SetVal();
byte dht11_check_sum = dht11_dat[0] + dht11_dat[1] + dht11_dat[2]
+ dht11_dat[3];
// check check_sum
if (dht11_dat[4] != dht11_check_sum) {
printf("DHT11 checksum error\r\n");
}
printf("Current humdity = ");
//printf("%d",dht11_dat[0]);
UTIL1_Num8uToStr(strTmp1, sizeof(strTmp1),dht11_dat[0]);
UTIL1_Num8uToStr(strTmp2, sizeof(strTmp2),dht11_dat[1]);
UTIL1_strcat(strTmp1, sizeof(strTmp1),(byte *)".");
UTIL1_strcat(strTmp1, sizeof(strTmp1),strTmp2);
UTIL1_strcat(strTmp1, sizeof(strTmp1),(byte *)"%%\r\n");
//printf((char *)strTmp1);
//printf(".");
//printf((char *)strTmp2);
//printf("%d",dht11_dat[1]);
//printf("%%\r\n");
printf((char *)strTmp1);
printf("temperature = ");
printf("%d",dht11_dat[2]);
printf(".");
printf("%d",dht11_dat[3]);
printf("C \r\n");
// WAIT1_Waitms(2000);//delay(2000);
}
static uint8_t ESP_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[48];
CLS1_SendStatusStr("ESP8266", "\r\n", io->stdOut);
CLS1_SendStatusStr(" Webserver", ESP_WebServerIsOn?"ON\r\n":"OFF\r\n", io->stdOut);
if (ESP_GetFirmwareVersionString(buf, sizeof(buf)) != ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
} else {
/* 00160901: 0016 is SDK version, 0901 is the AT version */
UTIL1_strcat(buf, sizeof(buf), "\r\n");
}
CLS1_SendStatusStr(" AT+GMR", buf, io->stdOut);
if (ESP_GetModeString(buf, sizeof(buf)) != ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
} else {
if (UTIL1_strcmp(buf, "1")==0) {
UTIL1_strcat(buf, sizeof(buf), " (device)");
} else if (UTIL1_strcmp(buf, "2")==0) {
UTIL1_strcat(buf, sizeof(buf), " (AP)");
} else if (UTIL1_strcmp(buf, "3")==0) {
UTIL1_strcat(buf, sizeof(buf), " (device+AP)");
} else {
UTIL1_strcat(buf, sizeof(buf), " (ERROR)");
}
UTIL1_strcat(buf, sizeof(buf), "\r\n");
}
CLS1_SendStatusStr(" AT+CWMODE?", buf, io->stdOut);
if (ESP_GetIPAddrString(buf, sizeof(buf)) != ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
} else {
UTIL1_strcat(buf, sizeof(buf), "\r\n");
}
CLS1_SendStatusStr(" AT+CIFSR", buf, io->stdOut);
if (ESP_GetConnectedAPString(buf, sizeof(buf)) != ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
} else {
UTIL1_strcat(buf, sizeof(buf), "\r\n");
}
CLS1_SendStatusStr(" AT+CWJAP?", buf, io->stdOut);
if (ESP_GetCIPMUXString(buf, sizeof(buf)) != ERR_OK) {
UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
} else {
if (UTIL1_strcmp(buf, "0")==0) {
UTIL1_strcat(buf, sizeof(buf), " (single connection)");
} else if (UTIL1_strcmp(buf, "1")==0) {
UTIL1_strcat(buf, sizeof(buf), " (multiple connections)");
} else {
UTIL1_strcat(buf, sizeof(buf), " (ERROR)");
}
UTIL1_strcat(buf, sizeof(buf), "\r\n");
}
CLS1_SendStatusStr(" CIPMUX", buf, io->stdOut);
return ERR_OK;
}
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);
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, toggle remote*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
DRV_SetMode(DRV_MODE_SPEED);
} else if (val=='G') { /* center joystick button: horn*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 2000 300");
} else if (val=='A')
{
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(STRAIGHT_HAND);
LF_StartFollowing();
} else if (val=='C') { /* 'C' button: motor stop*/
DRV_SetMode(DRV_MODE_STOP);
} else if (val=='B') { /* 'B' button: start right-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(RIGHT_HAND);
LF_StartFollowing();
} else if (val=='D') { /* 'D' button: start left-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(LEFT_HAND);
LF_StartFollowing();
} else if (val=='E') {
REF_CalibrateStartStop();
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t direction, speed;
int16_t x1000, y1000;
*handled = TRUE;
direction = *data; /* get x data value */
speed = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
//x1000 = scaleJoystickTo1K(x);
//y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, disable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
REMOTE_SetOnOff(FALSE);
DRV_SetSpeed(0,0); /* turn off motors */
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
REMOTE_SetOnOff(TRUE);
DRV_SetMode(DRV_MODE_SPEED);
SHELL_SendString("Remote ON\r\n");
} else if (val=='C') { /* red 'C' button */
/*! \todo add functionality */
} else if (val=='A') { /* green 'A' button */
/*! \todo add functionality */
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
