/* * Copyright (c) 2021, Erich Styger * * SPDX-License-Identifier: BSD-3-Clause */ #include "platform.h" #include "Application.h" #include "McuRTOS.h" #include "McuUtility.h" #if PL_HAS_ULTRASONIC #include "Ultrasonic.h" #endif #if PL_CONFIG_USE_LEDS #include "Leds.h" #include "McuLED.h" #endif #include "McuWait.h" #include "McuUtility.h" #include "Shell.h" #include "Motor.h" #if PL_CONFIG_USE_LINE_SENSOR #include "Reflectance.h" #endif #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE #include "LineFollow.h" #endif #include "Motor.h" #if PL_HAS_TSS #include "TSS1.h" #endif #if PL_CONFIG_USE_BUZZER #include "Buzzer.h" #endif #include "Turn.h" #if PL_HAS_USER_BUTTON #include "SW1.h" #endif #if PL_CONFIG_USE_LEDS #include "leds.h" #include "McuLED.h" #endif #if PL_CONFIG_APP_LINE_MAZE #include "Maze.h" #endif #if PL_HAS_RADIO #include "RStack.h" #endif #if PL_CONFIG_USE_REMOTE #include "Remote.h" #endif #if PL_HAS_SHELL_QUEUE #include "ShellQueue.h" #endif #if PL_HAS_DISTANCE_SENSOR #include "Distance.h" #endif #if PL_HAS_ACCEL_SENSOR #include "Accel.h" #include "MMA1.h" #endif #if PL_CONFIG_APP_SUMO #include "Sumo.h" #endif #if PL_CONFIG_USE_PID #include "PID.h" #endif #if PL_CONFIG_USE_EVENTS #include "Event.h" #endif #if PL_HAS_TEST #include "Test.h" #endif #if configUSE_TRACE_HOOKS #include "PTRC1.h" #endif #if PL_CONFIG_USE_TACHO #include "Tacho.h" #endif #if PL_HAS_RADIO #include "Radio.h" #include "RNet_App.h" #endif #if PL_HAS_SHELL_TRACE #include "ShellTrace.h" #endif #if PL_HAS_FREEMASTER #include "FMSTR1.h" #endif #if PL_CONFIG_USE_DRIVE #include "Drive.h" #endif #if PL_CONFIG_USE_I2C_SPY #include "I2CSPY1.h" #endif #if PL_HAS_MCP4728 #include "MCP4728.h" #endif #if PL_CONFIG_HAS_BATTERY_ADC #include "Battery.h" #endif #if PL_HAS_ROBO_SHIELD #include "RoboShield.h" #endif #if PL_HAS_MUSIC_SHIELD #include "Music.h" #endif #if PL_HAS_SPI #include "SPI.h" #endif #if PL_HAS_LASER #include "Laser.h" #endif #if PL_CONFIG_USE_QUADRATURE #include "QuadCounter.h" #endif #if PL_CONFIG_USE_IDENTIFY #include "Identify.h" #endif #if PL_HAS_MINT #include "MintRobot.h" #endif #if PL_HAS_MINT_REMOTE #include "MintRemote.h" #endif #if PL_HAS_MINT_LED #include "MintLed.h" #endif #if PL_HAS_TOF_SENSOR #include "VL6180X.h" #endif #if PL_CONFIG_HAS_NVM_CONFIG #include "NVM_Config.h" #endif #if PL_CONFIG_HAS_LCD #include "LCD.h" #include "GDisp1.h" #endif #include "McuEvents.h" #if PL_CONFIG_HAS_DEBOUNCE #include "KeyDebounce.h" #endif #include "McuArmTools.h" #if PL_HAS_MIDI #include "MidiMusic.h" #endif #if PL_CONFIG_USE_ESP32 #include "McuESP32.h" #endif typedef enum { APP_STATE_STARTUP, APP_STATE_INIT, #if PL_CONFIG_USE_LINE_SENSOR APP_STATE_CALIBRATE, #endif #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE APP_STATE_FOLLOW_LINE, #endif #if PL_APP_FOLLOW_OBSTACLE APP_STATE_FOLLOW_OBSTACLE, /* follow obstacle */ #endif #if PL_DO_MIDI APP_STATE_PLAY_MIDI, #endif #if PL_CONFIG_APP_SUMO APP_STATE_RUN_SUMO, /* Sumo fight */ #endif APP_STATE_IDLE, APP_STATE_MANUAL_MOVE, APP_STATE_AUTO, } AppStateType; static AppStateType appState = APP_STATE_STARTUP; #if PL_APP_FOLLOW_OBSTACLE static bool followObstacle = FALSE; #endif #if PL_APP_AVOID_OBSTACLE static bool avoidObstacle = FALSE; #endif void APP_DebugPrint(unsigned char *str) { #if PL_CONFIG_USE_SHELL SHELL_SendString(str); #endif } #if PL_CONFIG_USE_SHELL static unsigned char *AppStateString(AppStateType state) { switch(state) { case APP_STATE_STARTUP: return (unsigned char*)"STARTUP"; case APP_STATE_INIT: return (unsigned char*)"INIT"; #if PL_CONFIG_USE_LINE_SENSOR case APP_STATE_CALIBRATE: return (unsigned char*)"CALIBRATE"; #endif #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE case APP_STATE_FOLLOW_LINE: return (unsigned char*)"FOLLOW_LINE"; #endif #if PL_APP_FOLLOW_OBSTACLE case APP_STATE_FOLLOW_OBSTACLE: return (unsigned char*)"FOLLOW_OBSTACLE"; #endif #if PL_DO_MIDI case APP_STATE_PLAY_MIDI: return (unsigned char*)"PLAY_MIDI"; #endif #if PL_CONFIG_APP_SUMO case APP_STATE_RUN_SUMO: return (unsigned char*)"RUN_SUMO"; #endif case APP_STATE_IDLE: return (unsigned char*)"IDLE"; default: break; } return (unsigned char*)"unknown?"; } #endif static void StateMachine(bool buttonPress) { #if PL_CONFIG_USE_LINE_SENSOR static uint8_t cnt; static uint8_t mazeFailure; static uint8_t presentCnt; #endif // TODO: send appstate via Mqtt switch (appState) { case APP_STATE_STARTUP: #if PL_CONFIG_USE_BUZZER && PL_DO_MINT BUZ_PlayTune(BUZ_TUNE_BUTTON); /* only short beep */ #elif PL_CONFIG_USE_BUZZER //BUZ_Beep(300, 800); //BUZ_PlayTune(BUZ_TUNE_WELCOME); BUZ_PlayTune(BUZ_TUNE_BUTTON); #endif #if PL_HAS_MUSIC_POWERUP McuWait_WaitOSms(300); MUSIC_PlayTheme(MUSIC_THEME_IMPERIAL); #endif appState = APP_STATE_INIT; break; case APP_STATE_INIT: #if PL_CONFIG_USE_LEDS McuLED_On(LEDS_Left); McuLED_Off(LEDS_Right); #endif #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE || PL_CONFIG_APP_SUMO if (REF_CanUseSensor()) { appState = APP_STATE_IDLE; } if (buttonPress) { APP_StateStartCalibrate(); } #elif PL_APP_FOLLOW_OBSTACLE appState = APP_STATE_IDLE; #endif break; #if PL_APP_FOLLOW_OBSTACLE case APP_STATE_FOLLOW_OBSTACLE: cnt++; if (cnt>50) { cnt = 0; #if PL_HAS_LED_RED McuLED_Toggle(LEDS_Left); #endif } if (buttonPress) { followObstacle = FALSE; appState = APP_STATE_IDLE; } break; #endif #if PL_CONFIG_USE_LINE_SENSOR case APP_STATE_CALIBRATE: cnt++; if (cnt>50) { cnt = 0; #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Left); #endif } if (buttonPress) { APP_StateStopCalibrate(); } break; #endif case APP_STATE_IDLE: #if PL_CONFIG_USE_LEDS //McuLED_Off(LEDS_Left); //McuLED_On(LEDS_Right); #endif if (buttonPress) { #if (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) && !PL_APP_MINT /* Mint application handles buttons itself */ if (LF_IsFollowing()) { // LF_StopFollowing(); // appState = APP_STATE_IDLE; } else { // LF_StartFollowing(); // appState = APP_STATE_FOLLOW_LINE; } #elif PL_APP_FOLLOW_OBSTACLE followObstacle = TRUE; appState = APP_STATE_FOLLOW_OBSTACLE; #elif 0 && PL_DO_MIDI MM_PlayMusic(-1); /* play current song */ appState = APP_STATE_PLAY_MIDI; #elif PL_CONFIG_APP_SUMO SUMO_StartCountdown(); appState = APP_STATE_RUN_SUMO; #endif #if PL_HAS_MUSIC_START_SUMO MUSIC_PlayTheme(MUSIC_THEME_DARK_SIDE); #endif } break; #if 0 && PL_DO_MIDI case APP_STATE_PLAY_MIDI: if (buttonPress || !MM_IsPlaying()) { MM_StopPlaying(); appState = APP_STATE_IDLE; } break; #endif #if PL_CONFIG_APP_SUMO case APP_STATE_RUN_SUMO: #if PL_IS_INTRO_ZUMO_K22 || PL_IS_INTRO_ZUMO_K22_V2 /* no LED signaling */ #else #if PL_CONFIG_USE_LEDS McuLED_Off(LEDS_Left); McuLED_Off(LEDS_Right); #endif #endif if (!SUMO_IsDoingSumo()) { appState = APP_STATE_IDLE; } else { #if PL_HAS_MUSIC_RUN_SUMO if (!MUSIC_IsPlaying()) { MUSIC_PlayTheme(MUSIC_THEME_BREATHING); } #endif } if (buttonPress) { SUMO_StopSumo(); appState = APP_STATE_IDLE; } break; #endif #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE case APP_STATE_FOLLOW_LINE: #if PL_CONFIG_USE_LEDS McuLED_Off(LEDS_Left); McuLED_Off(LEDS_Right); #endif // if (!LF_IsFollowing()) { // appState = APP_STATE_IDLE; // } if (buttonPress) { LF_StopFollowing(); appState = APP_STATE_IDLE; } break; #endif case APP_STATE_MANUAL_MOVE: if(REF_GetLineKind(REF_LINE_KIND_MODE_MAZE) == REF_LINE_STRAIGHT){ // send to mqtt AUTO // SHELL_SendString((unsigned char*)"AUTO!!!\r\n"); mazeFailure = 0; MAZE_ResetPresentCount(); presentCnt = 0; MAZE_ClearSolution(); LF_StartFollowing(); // BUZ_Beep(2000, 1000); BUZ_PlayTune(BUZ_TUNE_WELCOME); McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"\r\n", McuESP32_GetTxToESPStdio()->stdOut); appState = APP_STATE_AUTO; // McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); } vTaskDelay(pdMS_TO_TICKS(10)); break; case APP_STATE_AUTO: if(!LF_IsFollowing() && MAZE_IsSolved()){ //done SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n"); McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FINAL\"\r\n", McuESP32_GetTxToESPStdio()->stdOut); // McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); appState = APP_STATE_IDLE; mazeFailure=0; }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){ // failed mazeFailure++; MAZE_ClearSolution(); DRV_SetSpeed(0, 0); DRV_SetMode(DRV_MODE_SPEED); LF_StartFollowing(); }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){ BUZ_Beep(1000, 1000); McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FAILURE\"\r\n", McuESP32_GetTxToESPStdio()->stdOut); // McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); appState = APP_STATE_IDLE; } if(MAZE_GetPresentCount() != presentCnt){ presentCnt = MAZE_GetPresentCount(); //publish BUZ_Beep(1000, 1000); unsigned char payload[50] = "mqtt publish \"/mobile/status/present/\" \""; unsigned char number[4] = ""; McuUtility_Num8uToStr(number, sizeof(number), presentCnt); McuUtility_strcat(payload, sizeof(payload), number); McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"\r\n"); McuShell_SendStr(payload, McuESP32_GetTxToESPStdio()->stdOut); // McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); } vTaskDelay(pdMS_TO_TICKS(10)); break; default: break; } /* switch */ } void APP_StateStartCalibrate(void) { #if PL_CONFIG_USE_LINE_SENSOR REF_CalibrateStartStop(); appState = APP_STATE_CALIBRATE; #endif } bool APP_StateIsCalibrating(void) { #if PL_CONFIG_USE_LINE_SENSOR return appState == APP_STATE_CALIBRATE; #else return FALSE; #endif } void APP_StateStopCalibrate(void) { appState = APP_STATE_IDLE; #if PL_CONFIG_USE_LINE_SENSOR REF_CalibrateStartStop(); #endif } #if PL_APP_AVOID_OBSTACLE static void AvoidObstacle(void) { if (DIST_10cmLeftOn() && DIST_5cmLeftOn() && !DIST_10cmRightOn() && !DIST_5cmRightOn()) { /* turn right */ TURN_TurnAngle(10); } else if (!DIST_10cmLeftOn() && !DIST_5cmLeftOn() && DIST_10cmRightOn() && DIST_5cmRightOn()) { /* turn left */ TURN_TurnAngle(-10); } } #endif #if PL_APP_FOLLOW_OBSTACLE static void FollowObstacle(void) { #define DUTY_SLOW 16 #define DUTY_MEDIUM 20 #define DUTY_FAST 23 static uint8_t cnt; //uint16_t cm, us; cnt++; /* get called with 100 Hz, reduce to 10 Hz */ if (cnt==10) { //us = US_Measure_us(); cnt = 0; if (followObstacle) { cm = US_usToCentimeters(us, 22); if (cm<10) { /* back up! */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -DUTY_SLOW); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -DUTY_SLOW); } else if (cm>=10 && cm<=20) { /* stand still */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); //TURN_Turn(TURN_RIGHT45); /* try to avoid obstacle */ } else if (cm>20 && cm<=40) { /* forward slowly */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_MEDIUM); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_MEDIUM); } else if (cm>40 && cm<60) { /* forward fast */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_FAST); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_FAST); } else { /* nothing in range */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); } } } } #endif #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN static uint8_t AutoCalibrateReflectance(void) { #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"start auto-calibration...\r\n"); #endif /* perform automatic calibration */ APP_StateStartCalibrate(); TURN_Turn(TURN_LEFT90, NULL); McuWait_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_RIGHT90, NULL); McuWait_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_RIGHT90, NULL); McuWait_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_LEFT90, NULL); McuWait_WaitOSms(500); /* wait some time */ TURN_Turn(TURN_STOP, NULL); APP_StateStopCalibrate(); #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"auto-calibration finished.\r\n"); #endif return ERR_OK; } #endif static uint8_t SetManualMode(){ appState = APP_STATE_MANUAL_MOVE; McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"MANUAL\"\r\n", McuESP32_GetTxToESPStdio()->stdOut); return ERR_OK; } #if 0 /* do not use this any more, use proper debouncing! */ static void CheckButton(void) { #if PL_HAS_USER_BUTTON uint32_t timeTicks; /* time in ticks */ #define BUTTON_CNT_MS 100 /* iteration count for button */ #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN bool autoCalibrate = FALSE; #endif if (SW1_GetVal()==0) { /* button pressed */ /* short press (1 beep): start or stop line following if calibrated * 1 s press (2 beep): calibrate manually * 2 s press (3 beep): calibrate with auto-move * 3 s press (4 beep or more): clear path * */ vTaskDelay(50/portTICK_PERIOD_MS); /* simple debounce */ if (SW1_GetVal()==0) { /* still pressed */ #if PL_CONFIG_USE_LEDS McuLED_On(LEDS_Left); McuLED_On(LEDS_Right); #endif timeTicks = 0; while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) { vTaskDelay(BUTTON_CNT_MS/portTICK_PERIOD_MS); if ((timeTicks%(1000/BUTTON_CNT_MS))==0) { #if PL_CONFIG_USE_BUZZER (void)BUZ_Beep(300, 500); #elif PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif } timeTicks++; } /* wait until released */ #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN autoCalibrate = FALSE; #endif if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */ #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"button press.\r\n"); #endif StateMachine(TRUE); /* <1 s, short button press, according to state machine */ #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE || PL_CONFIG_APP_SUMO } else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) { #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"calibrate.\r\n"); #endif APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */ } else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) { #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"auto calibrate.\r\n"); #endif autoCalibrate = TRUE; /* 2-3 s: start auto calibration */ #endif #if PL_CONFIG_APP_LINE_MAZE } else if (timeTicks>=(3000/BUTTON_CNT_MS)) { MAZE_ClearSolution(); SHELL_SendString((unsigned char*)"Deleted solution.\r\n"); #if PL_CONFIG_USE_BUZZER McuWait_WaitOSms(500); (void)BUZ_Beep(300, 100); McuWait_WaitOSms(100); #endif #endif } while (SW1_GetVal()==0) { /* wait until button is released */ vTaskDelay(BUTTON_CNT_MS/portTICK_PERIOD_MS); } #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN if (autoCalibrate) { /* perform automatic calibration */ McuWait_WaitOSms(1500); /* wait some time so the user can remove the finger from the button */ (void)AutoCalibrateReflectance(); } #endif } } /* if */ #endif } #endif #if PL_HAS_ACCEL_STOP static void CheckAccelerometer(void) { int16_t x, y, z; bool isEnabled; uint8_t res; static uint8_t cnt = 0; #if PL_HAS_ACCEL_STOP_INVERTED /* board with sensor mounted head-over */ #define ACCEL_OK_CONDITION(x,y,z) ((z)<0) #else #define ACCEL_OK_CONDITION(x,y,z) (-900<=(z) && (z)<5000) #endif if ((cnt%10)==0) { /* check not too often */ res = ACCEL_isEnabled(&isEnabled); } else { res = ERR_OK; isEnabled = TRUE; } cnt++; if (res==ERR_OK && isEnabled) { ACCEL_GetValues(&x, &y, &z); if (!ACCEL_OK_CONDITION(x,y,z)) { /* measure again */ vTaskDelay(100/portTICK_PERIOD_MS); ACCEL_GetValues(&x, &y, &z); if (!ACCEL_OK_CONDITION(x,y,z)) { #if PL_CONFIG_USE_DRIVE DRV_Stop(1000); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif #if PL_DO_LINE_MAZE || PL_DO_LINE_FOLLOWING LF_StopFollowing(); #endif #if PL_DO_LINE_MAZE MAZE_ClearSolution(); #endif #if PL_CONFIG_APP_SUMO SUMO_Stop(); #endif #if PL_HAS_MUSIC_ACCEL_STOP MUSIC_PlayTheme(MUSIC_THEME_DAVE); #endif SHELL_SendString((unsigned char*)"Accel: Engines stopped!\r\n"); #if 1 { uint8_t buf[32]; McuUtility_strcpy(buf, sizeof(buf), "x: "); McuUtility_strcatNum16s(buf, sizeof(buf), x); McuUtility_strcat(buf, sizeof(buf), " y: "); McuUtility_strcatNum16s(buf, sizeof(buf), y); McuUtility_strcat(buf, sizeof(buf), " z: "); McuUtility_strcatNum16s(buf, sizeof(buf), z); McuUtility_strcat(buf, sizeof(buf), "\r\n"); SHELL_SendString(buf); } #endif #if PL_CONFIG_USE_BUZZER (void)BUZ_Beep(300, 500); #endif #if PL_HAS_MUSIC_ACCEL_STOP MUSIC_PlayTheme(MUSIC_THEME_DAVE); #endif } } } else { SHELL_SendString((unsigned char*)"Accelerometer disabled?\r\n"); (void)ACCEL_Enable(); } } #endif /* PL_HAS_ACCEL_STOP */ #if PL_CONFIG_USE_EVENTS static void MapButtonsForDisplay(EVNT_Handle event) { #if PL_CONFIG_HAS_LCD if (GDisp1_GetDisplayOrientation()==GDisp1_ORIENTATION_LANDSCAPE) { switch(event) { case EVNT_SW1_PRESSED: LCD_SetEvent(LCD_BTN_RIGHT); break; case EVNT_SW1_PRESSED_REPEAT: LCD_SetEvent(LCD_BTN_RIGHT_REPEAT); break; case EVNT_SW2_PRESSED: LCD_SetEvent(LCD_BTN_LEFT); break; case EVNT_SW2_PRESSED_REPEAT: LCD_SetEvent(LCD_BTN_LEFT_REPEAT); break; case EVNT_SW3_PRESSED: LCD_SetEvent(LCD_BTN_DOWN); break; case EVNT_SW3_PRESSED_REPEAT: LCD_SetEvent(LCD_BTN_DOWN_REPEAT); break; case EVNT_SW4_PRESSED: LCD_SetEvent(LCD_BTN_CENTER); break; case EVNT_SW4_PRESSED_REPEAT: LCD_SetEvent(LCD_BTN_CENTER_REPEAT); break; case EVNT_SW5_PRESSED: LCD_SetEvent(LCD_BTN_UP); break; case EVNT_SW5_PRESSED_REPEAT: LCD_SetEvent(LCD_BTN_UP_REPEAT); break; case EVNT_SW6_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_DOWN); break; case EVNT_SW6_PRESSED_REPEAT: LCD_SetEvent(LCD_SIDE_BTN_DOWN_REPEAT); break; case EVNT_SW7_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_UP); break; case EVNT_SW7_PRESSED_REPEAT: LCD_SetEvent(LCD_SIDE_BTN_UP_REPEAT); break; case EVNT_SW1_RELEASED: case EVNT_SW1_LRELEASED: case EVNT_SW2_RELEASED: case EVNT_SW2_LRELEASED: case EVNT_SW3_RELEASED: case EVNT_SW3_LRELEASED: case EVNT_SW4_RELEASED: case EVNT_SW4_LRELEASED: case EVNT_SW5_RELEASED: case EVNT_SW5_LRELEASED: case EVNT_SW6_RELEASED: case EVNT_SW6_LRELEASED: case EVNT_SW7_RELEASED: case EVNT_SW7_LRELEASED: LCD_SetEvent(LCD_BTN_RELEASED); break; default: break; } /* case */ } else { switch(event) { case EVNT_SW1_PRESSED: LCD_SetEvent(LCD_BTN_LEFT); break; case EVNT_SW2_PRESSED: LCD_SetEvent(LCD_BTN_RIGHT); break; case EVNT_SW3_PRESSED: LCD_SetEvent(LCD_BTN_UP); break; case EVNT_SW4_PRESSED: LCD_SetEvent(LCD_BTN_CENTER); break; case EVNT_SW5_PRESSED: LCD_SetEvent(LCD_BTN_DOWN); break; case EVNT_SW6_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_UP); break; case EVNT_SW7_PRESSED: LCD_SetEvent(LCD_SIDE_BTN_DOWN); break; default: break; } /* case */ } #else (void)event; /* not used */ #endif } #endif /* PL_CONFIG_USE_EVENTS */ #if PL_CONFIG_USE_EVENTS static void APP_EventHandler(EVNT_Handle event) { MapButtonsForDisplay(event); switch(event) { case EVNT_STARTUP: #if PL_CONFIG_USE_LEDS McuLED_On(LEDS_Left); /* just do something */ #endif #if PL_CONFIG_HAS_BUZZER BUZ_PlayTune(BUZ_TUNE_WELCOME); #endif break; case EVNT_LED_HEARTBEAT: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Left); #endif break; #if PL_CONFIG_USE_BUTTONS #if PL_CONFIG_NOF_BUTTONS>=1 case EVNT_SW1_PRESSED: #if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE LF_StartStopFollowing(); #endif #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW1 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('a'); /* A */ #endif #if PL_DO_MIDI if (MM_IsPlaying()) { MM_StopPlaying(); } else { MM_Play(1); } #elif PL_CONFIG_APP_SUMO #if PL_CONFIG_HAS_BUZZER BUZ_PlayTune(BUZ_TUNE_BUTTON); #endif if (!SUMO_IsDoingSumo()) { SUMO_StartCountdown(); } else { SUMO_StopSumo(); } #endif break; case EVNT_SW1_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW1 repeat\r\n"); break; case EVNT_SW1_RELEASED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW1 released\r\n"); break; case EVNT_SW1_LPRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW1 long pressed\r\n"); #if PL_CONFIG_HAS_BUZZER BUZ_PlayTune(BUZ_TUNE_BUTTON_LONG); #endif break; case EVNT_SW1_LRELEASED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW1 long released\r\n"); #if PL_CONFIG_HAS_BUZZER BUZ_PlayTune(BUZ_TUNE_BUTTON_LONG); #endif break; #endif /* PL_CONFIG_NOF_BUTTONS>=1 */ #if PL_CONFIG_NOF_BUTTONS>=2 case EVNT_SW2_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW2 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('b'); /* B */ #endif break; case EVNT_SW2_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW2 repeat\r\n"); break; case EVNT_SW2_LPRESSED: case EVNT_SW2_RELEASED: break; #endif #if PL_CONFIG_NOF_BUTTONS>=3 case EVNT_SW3_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW3 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('c'); /* C */ #endif break; case EVNT_SW3_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW3 repeat\r\n"); break; case EVNT_SW3_LPRESSED: case EVNT_SW3_RELEASED: break; #endif #if PL_CONFIG_NOF_BUTTONS>=4 case EVNT_SW4_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW4 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('d'); /* D */ #endif break; case EVNT_SW4_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW4 repeat\r\n"); break; case EVNT_SW4_LPRESSED: case EVNT_SW4_RELEASED: break; #endif #if PL_CONFIG_NOF_BUTTONS>=5 case EVNT_SW5_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW5 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('e'); #endif break; case EVNT_SW5_LPRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW5L pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('E'); #endif break; case EVNT_SW5_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW5 repeat\r\n"); break; case EVNT_SW5_RELEASED: break; #endif #if PL_CONFIG_NOF_BUTTONS>=6 case EVNT_SW6_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW6 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('f'); #endif break; case EVNT_SW6_LPRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW6L pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('F'); #endif break; case EVNT_SW6_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW6 repeat\r\n"); break; case EVNT_SW6_RELEASED: break; #endif #if PL_CONFIG_NOF_BUTTONS>=7 case EVNT_SW7_PRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW7 pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('g'); #endif break; case EVNT_SW7_LPRESSED: #if PL_CONFIG_USE_LEDS McuLED_Toggle(LEDS_Right); #endif SHELL_SendString((uint8_t*)"SW7L pressed\r\n"); #if PL_CONFIG_CONTROL_SENDER (void)REMOTE_SendButton('G'); /* G */ #endif break; case EVNT_SW7_PRESSED_REPEAT: SHELL_SendString((uint8_t*)"SW7 repeat\r\n"); break; case EVNT_SW7_RELEASED: break; #endif #endif default: break; } /* switch */ } #endif /* PL_CONFIG_USE_EVENTS */ static void MainTask(void *pvParameters) { #if PL_HAS_ACCEL_STOP || (PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS) uint32_t cnt = 0; #endif (void)pvParameters; /* not used */ vTaskDelay(pdMS_TO_TICKS(100)); /* wait some time to get hardware (SW1) pull-up effective */ #if PL_HAS_ACCEL_SENSOR if (ACCEL_LowLevelInit()!=ERR_OK) { /* always enable accelerometer */ #if PL_CONFIG_USE_SHELL SHELL_SendString((unsigned char*)"ERR: Accel sensor init failed!\r\n"); #endif } #endif #if PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS McuLED_On(LEDS_Left); #endif #if PL_HAS_TSS Configure(); /* initialize TSS library */ #endif #if PL_HAS_MINT_LED MINTLED_SetMode(MINTLED_MODE_WAIT_BALL_REMOVAL); /* initial demo mode */ #endif for(;;) { #if PL_CONFIG_HAS_DEBOUNCE KEYDBNC_Process(); /* scan and debounce polled keys */ #endif #if PL_CONFIG_USE_EVENTS EVNT_HandleEvent(APP_EventHandler, TRUE); #endif #if PL_HAS_TSS TSS_Task(); /* call TSS library to process touches */ #elif 0 && !PL_DO_TEST CheckButton(); #endif #if PL_APP_AVOID_OBSTACLE AvoidObstacle(); #endif #if PL_APP_FOLLOW_OBSTACLE FollowObstacle(); #endif StateMachine(FALSE); #if 0 && PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS if ((cnt%48)==0) { McuLED_Toggle(LEDS_Left); } #endif #if PL_HAS_FREEMASTER FMSTR1_Poll(); FMSTR1_Recorder(); #endif #if PL_HAS_ACCEL_STOP if ((cnt%10)==0) { /* every 100 ms */ CheckAccelerometer(); } #endif /* PL_HAS_ACCEL_STOP */ #if PL_HAS_LASER { bool isLeft, isRight; LASER_LeftRightStatus(&isLeft, &isRight); USER_LED2_Put(isLeft); USER_LED_Put(isRight); } #endif #if PL_HAS_ACCEL_STOP || (PL_CONFIG_APP_SUMO && PL_CONFIG_USE_LEDS) cnt++; #endif vTaskDelay(10/portTICK_PERIOD_MS); } /* for */ } #if PL_CONFIG_USE_SHELL static uint8_t APP_PrintHelp(const McuShell_StdIOType *io) { McuShell_SendHelpStr((unsigned char*)"app", (unsigned char*)"Group of app commands\r\n", io->stdOut); #if PL_DO_MINT McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"Short press (1 beep): enter automatic mode, simulate ball received/removed.\r\n", io->stdOut); #elif PL_IS_ROBOT McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"Short press (1 beep): start or stop line following if calibrated\r\n", io->stdOut); McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"1 s press (2 beep): calibrate manually\r\n", io->stdOut); McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"2 s press (3 beep): calibrate with auto-move\r\n", io->stdOut); McuShell_SendHelpStr((unsigned char*)"", (unsigned char*)"3 s press (4 beep or more): clear path\r\n", io->stdOut); #endif McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows app help or status\r\n", io->stdOut); #if PL_APP_FOLLOW_OBSTACLE McuShell_SendHelpStr((unsigned char*)" follow (on|off)", (unsigned char*)"Obstacle following on or off\r\n", io->stdOut); #endif #if PL_APP_AVOID_OBSTACLE McuShell_SendHelpStr((unsigned char*)" avoid (on|off)", (unsigned char*)"Obstacle avoidance on or off\r\n", io->stdOut); #endif #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN McuShell_SendHelpStr((unsigned char*)" autocalib", (unsigned char*)"Automatically calibrate line sensor\r\n", io->stdOut); #endif //app manualdrive McuShell_SendHelpStr((unsigned char*)" manualdrive", (unsigned char*)"Robot can be driven manualy\r\n", io->stdOut); return ERR_OK; } static uint8_t APP_PrintStatus(const McuShell_StdIOType *io) { McuShell_SendStatusStr((unsigned char*)"app", (unsigned char*)"Application status\r\n", io->stdOut); #if PL_CONFIG_APP_SUMO McuShell_SendStatusStr((unsigned char*)" mode", (unsigned char*)"SUMO\r\n", io->stdOut); #endif #if PL_APP_REMOTE_CONTROL McuShell_SendStatusStr((unsigned char*)" mode", (unsigned char*)"REMOTE CONTROL\r\n", io->stdOut); #endif #if PL_CONFIG_APP_LINE_FOLLOWING McuShell_SendStatusStr((unsigned char*)" mode", (unsigned char*)"LINE FOLLOWING\r\n", io->stdOut); #endif #if PL_CONFIG_APP_LINE_MAZE McuShell_SendStatusStr((unsigned char*)" mode", (unsigned char*)"LINE MAZE\r\n", io->stdOut); #endif #if PL_APP_FOLLOW_OBSTACLE McuShell_SendStatusStr((unsigned char*)" follow", followObstacle?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut); #endif #if PL_HAS_USER_BUTTON McuShell_SendStatusStr((unsigned char*)" button", SW1_GetVal()==0?(unsigned char*)"pressed\r\n":(unsigned char*)"released\r\n", io->stdOut); #endif McuShell_SendStatusStr((unsigned char*)" App State", AppStateString(appState), io->stdOut); McuShell_SendStr((unsigned char*)"\r\n", io->stdOut); return ERR_OK; } uint8_t APP_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) { uint8_t res = ERR_OK; if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"app help")==0) { *handled = TRUE; return APP_PrintHelp(io); } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"app status")==0) { *handled = TRUE; return APP_PrintStatus(io); }else if(McuUtility_strcmp((char*)cmd, (char*)"app manualdrive")==0){ *handled = TRUE; return SetManualMode(); #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN } else if (McuUtility_strcmp((char*)cmd, (char*)"app autocalib")==0) { *handled = TRUE; return AutoCalibrateReflectance(); #endif #if PL_APP_AVOID_OBSTACLE } else if (McuUtility_strcmp((char*)cmd, (char*)"app avoid on")==0) { avoidObstacle = TRUE; *handled = TRUE; } else if (McuUtility_strcmp((char*)cmd, (char*)"app avoid off")==0) { avoidObstacle = FALSE; *handled = TRUE; #endif #if PL_APP_FOLLOW_OBSTACLE } else if (McuUtility_strcmp((char*)cmd, (char*)"app follow on")==0) { followObstacle = TRUE; *handled = TRUE; } else if (McuUtility_strcmp((char*)cmd, (char*)"app follow off")==0) { followObstacle = FALSE; *handled = TRUE; #endif } return res; } #endif /* PL_CONFIG_USE_SHELL */ #if configUSE_HEAP_SCHEME==5 static __attribute__ ((used,section(".noinit.$SRAM_LOWER.Heap5"))) uint8_t heap_sram_lower[64*1024]; /* placed in in no_init section inside SRAM_LOWER */ static __attribute__ ((used,section(".noinit_Heap5"))) uint8_t heap_sram_upper[16*1024]; /* placed in in no_init section inside SRAM_UPPER */ static const HeapRegion_t xHeapRegions[] = { { &heap_sram_lower[0], sizeof(heap_sram_lower)}, { &heap_sram_upper[0], sizeof(heap_sram_upper)}, { NULL, 0 } /* << Terminates the array. */ }; #endif void APP_Run(void) { #if configUSE_HEAP_SCHEME==5 vPortDefineHeapRegions(xHeapRegions); /* Pass the array into vPortDefineHeapRegions(). Must be called first! */ #endif PL_Init(); appState = APP_STATE_STARTUP; if (xTaskCreate(MainTask, "Main", 600/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+1, NULL) != pdPASS) { for(;;){} /* error */ } vTaskStartScheduler(); }