void ICACHE_FLASH_ATTR
led_object_pack(PARAMS * params)
{
if(NULL == params)
{
PRINTF("Create first tlv param failed.\n");
return;
}
struct led* led = create_led();
led_get(led);
if (add_next_uint8(params, led->red))
{
PRINTF("Add next tlv param failed.\n");
return;
}
if (add_next_uint8(params, led->green))
{
PRINTF("Add next tlv param failed.\n");
return;
}
if (add_next_uint8(params, led->blue))
{
PRINTF("Add next tlv param failed.\n");
return;
}
delete_led(led);
}
/**
****************************************************************************************
* @brief Led 1 flash process
****************************************************************************************
*/
static void usr_led1_process(void)
{
if(led_get(1) == LED_ON)
{
led_set(1, LED_OFF);
ke_timer_set(APP_SYS_LED_1_TIMER, TASK_APP, usr_env.led1_off_dur);
}
else
{
led_set(1, LED_ON);
ke_timer_set(APP_SYS_LED_1_TIMER, TASK_APP, usr_env.led1_on_dur);
}
}
/* timer1の割り込み処理 */
void __attribute__((__interrupt__, __shadow__)) _T1Interrupt(void)
{
#if 0
static int count = 0;
// 0.5秒間隔のLEDの点滅(青)
if (count ++ % 500 == 0) {
led_set(1, !led_get(1));
}
#endif
// roombaの更新処理
roomba_update ();
// 割り込みフラグをクリア
IFS0bits.T1IF = 0;
}
/**
* roombaからのセンサ情報の受信
* 15ms間隔で受信する
*/
void roomba_update_recv (void)
{
char buff[128];
int i, size;
char c;
/* センサデータが来てるか調べるカウンタ */
roomba_sensor_none_count ++;
/* RoobmaのUARTの受信処理 */
for (;;) {
/* Group100は80+4byte:head+size+id+data[80]+check_sum */
/* Group6は52+4byte:head+size+id+data[52]+check_sum */
/* Group1は10+4byte:head+size+id+data[10]+check_sum */
if (uart2_recv_get_size () < 56) {
break;
}
/* streamのヘッダを設定する */
c = uart2_recv_get_char ();
if (roi_set_stream_response_header(buff, c) != 0) {
continue;
}
/* サイズを受信 */
c = uart2_recv_get_char ();
if (roi_set_stream_response_size (buff, c) != 0) {
continue;
}
/* データを受信 */
size = roi_get_stream_response_size (buff);
for (i=0; i<size; i++) {
c = uart2_recv_get_char();
roi_set_stream_response_data (buff, i, c);
}
roomba_sensor_count ++;
roomba_sensor_none_count = 0;
/* センサデータのコピー */
memcpy(roomba_stream_buff, buff, sizeof(roomba_stream_buff));
led_set (0, !led_get(0));
break;
}
}
/**
* roombaへのコマンドの送信
* 15msに1回ずつ送信する
*/
void roomba_update_send (void)
{
int size;
static unsigned char buff[64];
static int reactive_count;
/* センサデータが来ない場合 */
if (roomba_sensor_none_count > 1000) {
roomba_sensor_none_count = 0;
led_set (0, !led_get(0));
/* STREAMコマンドを送る*/
roomba_send_stream ();
}
/* 1秒おきにCONTROLコマンド送信(あとで変更) */
if (reactive_count % 1000 == 0) {
size = roi_set_start (buff);
uart2_send_data (buff, size);
size = roi_set_control (buff);
uart2_send_data (buff, size);
}
/* 50ms周期で送信 */
if (reactive_count % 50 == 0) {
/* Andoridから指令が来てない場合 */
if (!roomba_comm_flag) {
roomba_set_drive (0, 0);
}
/* センサに応じた回避行動 */
if (roi_get_stream_response_bump_l(roomba_stream_buff)) {
roomba_set_drive (-50, 0);
roomba_comm_flag = 0;
}
if (roi_get_stream_response_bump_r (roomba_stream_buff)) {
roomba_set_drive (-50, 0);
roomba_comm_flag = 0;
}
/* DRIVEコマンドの送信 */
size = roi_set_drive (buff, roomba_drive_v, roomba_drive_r);
uart2_send_data (buff, size);
}
reactive_count ++;
}
