diff --git a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp index 90023f7..8bd87e4 100644 --- a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp +++ b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp @@ -28,6 +28,9 @@ #include +void activate_solenoid(); +void deactivate_solenoid(); + /** * @struct TaskBoardDriver_v1 * @@ -282,8 +285,14 @@ struct TaskBoardDriver_v1 : }); Actuator* ball_drop_solenoid = new Actuator("BALL_DROP_SOLENOID", [&](Actuator::State state) { - // gpio_set_level(GPIO_NUM_27, state == Actuator::State::ON); - gpio_set_level(GPIO_NUM_27, 1); + if (state == Actuator::State::ON) + { + activate_solenoid(); + } + else + { + deactivate_solenoid(); + } }); Actuator* all_goal_leds = new Actuator("ALL_GOAL_LEDS", [&](Actuator::State state) { @@ -376,7 +385,7 @@ struct TaskBoardDriver_v1 : new TaskStepActuator(*goal_2_led, Actuator::State::OFF), new TaskStepActuator(*ball_drop_solenoid, Actuator::State::ON), new TaskStepActuator(*blue_button_led, Actuator::State::LED_ON), - new TaskStepEqualDuringRandom(*get_sensor_by_name("BLUE_BUTTON_LEFT"), SensorMeasurement(true), 0.0, 3000L, 8000L), + new TaskStepEqualDuringRandom(*get_sensor_by_name("BLUE_BUTTON_LEFT"), SensorMeasurement(true), 0.0, 1000L, 4000L), new TaskStepActuator(*ball_drop_solenoid, Actuator::State::OFF), new TaskStepActuator(*blue_button_led, Actuator::State::OFF), // do something here to assess if the ball has reached goal 1, maybe wait a fixed amount of time and return success if goal 1 is not made diff --git a/idf/taskboard/main/main_core0.cpp b/idf/taskboard/main/main_core0.cpp index a5ffafb..94d09ee 100644 --- a/idf/taskboard/main/main_core0.cpp +++ b/idf/taskboard/main/main_core0.cpp @@ -46,13 +46,12 @@ void websockets_task( void* args); /** - * @brief System entry point + * @brief GPIO configuration */ -extern "C" void app_main( - void) +void config_gpios() { - // Cycle the solenoid at start up esp_err_t ret_output; + // Set GPIO 19 to output mode ret_output = gpio_reset_pin(GPIO_NUM_19); if (ret_output != ESP_OK) { ESP_LOGE("GPIO", "Failed to reset pin 19"); @@ -63,6 +62,7 @@ extern "C" void app_main( ESP_LOGE("GPIO", "Failed to set pin 19 direction"); return; } + // Cycle the solenoid at start up ret_output = gpio_set_level(GPIO_NUM_19, 1); if (ret_output != ESP_OK) { ESP_LOGE("GPIO", "Failed to set pin 19 level"); @@ -74,6 +74,55 @@ extern "C" void app_main( ESP_LOGE("GPIO", "Failed to set pin 19 level"); return; } +} + +/** + * @brief Deactivate the solenoid + */ +void deactivate_solenoid() +{ + esp_err_t ret_output; + ret_output = gpio_set_level(GPIO_NUM_19, 0); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to set pin 19 level"); + return; + } +} + +TimerHandle_t timer_solenoid_ = xTimerCreate( //< Timer to turn solenoid off + "SolenoidOff", // Timer name + pdMS_TO_TICKS(5000), // Delay in ms + pdFALSE, // Disabled auto-reload + nullptr, + [](TimerHandle_t xTimer) { // Callback + // Turn solenoid off + deactivate_solenoid(); + } +); + +/** + * @brief Activate the solenoid for 5 seconds + */ +void activate_solenoid() +{ + esp_err_t ret_output; + ret_output = gpio_set_level(GPIO_NUM_19, 1); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to set pin 19 level"); + return; + } + if (timer_solenoid_ != nullptr) { + xTimerStart(timer_solenoid_, 0); + } +} + +/** + * @brief System entry point + */ +extern "C" void app_main( + void) +{ + config_gpios(); // ------------------------ // System Initialization