From f84f9c6f0056f01eaa26ba9a239c45083a6bcabd Mon Sep 17 00:00:00 2001 From: peterso Date: Sat, 3 May 2025 21:00:43 +0000 Subject: [PATCH 1/2] update sensor and actuator pin assignments to match as-built --- idf/taskboard/main/hal/PbHubController.hpp | 28 +++++++++ .../hal/board/TaskBoardDriver_TBv2025.hpp | 63 +++++++++++-------- 2 files changed, 65 insertions(+), 26 deletions(-) diff --git a/idf/taskboard/main/hal/PbHubController.hpp b/idf/taskboard/main/hal/PbHubController.hpp index 1360467..121b8a8 100644 --- a/idf/taskboard/main/hal/PbHubController.hpp +++ b/idf/taskboard/main/hal/PbHubController.hpp @@ -160,6 +160,34 @@ struct PbHubController read_operation(channel, Operation::WRITE_IO1, reinterpret_cast(&data), sizeof(data)); } + /** + * @brief Writes PWM value to IO0 pin + * + * @param channel Channel to write to + * @param value range 0 - 255 + */ + void write_PWM_IO0( + const Channel channel, + const uint8_t value) + { + uint8_t data = value; + read_operation(channel, Operation::PWM_IO0, reinterpret_cast(&data), sizeof(data)); + } + + /** + * @brief Writes PWM value to IO1 pin + * + * @param channel Channel to write to + * @param value range 0 - 255 + */ + void write_PWM_IO1( + const Channel channel, + const uint8_t value) + { + uint8_t data = value; + read_operation(channel, Operation::PWM_IO1, reinterpret_cast(&data), sizeof(data)); + } + protected: /** diff --git a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp index d6f24e3..27048e8 100644 --- a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp +++ b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp @@ -117,8 +117,8 @@ struct TaskBoardDriver_v1 : Sensor* blue_button_left = new Sensor("BLUE_BUTTON_LEFT", [&]() { bool value = - hardware_low_level_controller_.pb_hub_controller_1.read_digital_IO0(PbHubController::Channel:: - CHANNEL_4); + hardware_low_level_controller_.pb_hub_controller_2.read_digital_IO1(PbHubController::Channel:: + CHANNEL_0); return SensorMeasurement(!value); // Button is inverted }); @@ -126,8 +126,8 @@ struct TaskBoardDriver_v1 : Sensor* red_button_left = new Sensor("RED_BUTTON_LEFT", [&]() { bool value = - hardware_low_level_controller_.pb_hub_controller_1.read_digital_IO1(PbHubController::Channel:: - CHANNEL_4); + hardware_low_level_controller_.pb_hub_controller_2.read_digital_IO1(PbHubController::Channel:: + CHANNEL_0); return SensorMeasurement(!value); // Button is inverted }); @@ -135,8 +135,8 @@ struct TaskBoardDriver_v1 : Sensor* blue_button_right = new Sensor("BLUE_BUTTON_RIGHT", [&]() { bool value = - hardware_low_level_controller_.pb_hub_controller_1.read_digital_IO0(PbHubController::Channel:: - CHANNEL_5); + hardware_low_level_controller_.pb_hub_controller_2.read_digital_IO1(PbHubController::Channel:: + CHANNEL_1); return SensorMeasurement(!value); // Button is inverted }); @@ -144,8 +144,8 @@ struct TaskBoardDriver_v1 : Sensor* red_button_right = new Sensor("RED_BUTTON_RIGHT", [&]() { bool value = - hardware_low_level_controller_.pb_hub_controller_1.read_digital_IO1(PbHubController::Channel:: - CHANNEL_5); + hardware_low_level_controller_.pb_hub_controller_2.read_digital_IO1(PbHubController::Channel:: + CHANNEL_1); return SensorMeasurement(!value); // Button is inverted }); @@ -258,31 +258,40 @@ struct TaskBoardDriver_v1 : // Initialize actuators Actuator* goal_1_led = new Actuator("GOAL_1_LED", [&](Actuator::State state) { - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_0, state == Actuator::State::ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO0(PbHubController::Channel::CHANNEL_4, state == Actuator::State::LED_ON); }); Actuator* goal_2_led = new Actuator("GOAL_2_LED", [&](Actuator::State state) { - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_1, state == Actuator::State::ON); + ESP_LOGI("app_main", "setting value goal_2_led"); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO1(PbHubController::Channel::CHANNEL_4, state == Actuator::State::LED_ON); }); Actuator* goal_3_led = new Actuator("GOAL_3_LED", [&](Actuator::State state) { - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_2, state == Actuator::State::ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO0(PbHubController::Channel::CHANNEL_5, state == Actuator::State::LED_ON); }); Actuator* goal_4_led = new Actuator("GOAL_4_LED", [&](Actuator::State state) { - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_3, state == Actuator::State::ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO1(PbHubController::Channel::CHANNEL_5, state == Actuator::State::LED_ON); }); - 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, state == Actuator::State::ON); // was here when I started + // gpio_set_level(GPIO_NUM_27, 1); + gpio_set_level(GPIO_NUM_27, state); + + // if (state == Actuator::State::ON){ + // gpio_set_level(GPIO_NUM_27, 1); + // } else { + // gpio_set_level(GPIO_NUM_27, 0); + // } }); Actuator* all_goal_leds = new Actuator("ALL_GOAL_LEDS", [&](Actuator::State state) { - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_0, state == Actuator::State::ON); - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_1, state == Actuator::State::ON); - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_2, state == Actuator::State::ON); - hardware_low_level_controller_.pb_hub_controller_2.write_digital_IO0(PbHubController::Channel::CHANNEL_3, state == Actuator::State::ON); + ESP_LOGI("app_main", "setting values to all goal_leds"); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO0(PbHubController::Channel::CHANNEL_4, state == Actuator::State::LED_ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO1(PbHubController::Channel::CHANNEL_4, state == Actuator::State::LED_ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO0(PbHubController::Channel::CHANNEL_5, state == Actuator::State::LED_ON); + hardware_low_level_controller_.pb_hub_controller_1.write_PWM_IO1(PbHubController::Channel::CHANNEL_5, state == Actuator::State::LED_ON); }); actuators_.push_back(goal_1_led); @@ -339,37 +348,39 @@ struct TaskBoardDriver_v1 : new TaskStepActuator(*all_goal_leds, Actuator::State::OFF), new TaskStepActuator(*ball_drop_solenoid, Actuator::State::OFF), new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_1"), SensorMeasurement(true)), - new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON_RIGHT"), SensorMeasurement(true)), + new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_4"), SensorMeasurement(true)), + new TaskStepEqual(*get_sensor_by_name("RED_BUTTON_RIGHT"), SensorMeasurement(true)), }; default_precondition_task_ = new SimultaneousConditionTask(*precondition_steps, "Precondition Task"); std::vector* main_steps = new std::vector { + new TaskStepActuator(*all_goal_leds, Actuator::State::LED_ON), new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON_LEFT"), SensorMeasurement(true)), new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON_LEFT"), SensorMeasurement(false)), new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON_RIGHT"), SensorMeasurement(true)), + new TaskStepActuator(*all_goal_leds, Actuator::State::OFF), new TaskStepTraceShapeFromPool(*get_sensor_by_name("TOUCH_SCREEN"), shape_pool), new TaskStepTraceShapeFromPool(*get_sensor_by_name("TOUCH_SCREEN"), shape_pool), new TaskStepTraceShapeFromPool(*get_sensor_by_name("TOUCH_SCREEN"), shape_pool), - new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), - new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), - new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), - new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), - new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), + new TaskStepTouchGoalFromPool(*get_sensor_by_name("TOUCH_SCREEN"), touch_goal_pool), // PS: task needs to be fixed touches aren't registered + new TaskStepActuator(*goal_2_led, Actuator::State::ON), new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_2"), SensorMeasurement(true)), + new TaskStepActuator(*goal_2_led, Actuator::State::OFF), new TaskStepActuator(*ball_drop_solenoid, Actuator::State::ON), new TaskStepEqualDuringRandom(*get_sensor_by_name("BLUE_BUTTON_LEFT"), SensorMeasurement(true), 0.0, 3000L, 8000L), new TaskStepActuator(*ball_drop_solenoid, Actuator::State::OFF), new TaskStepActuator(*all_goal_leds, Actuator::State::ON), new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_1"), SensorMeasurement(true)), new TaskStepActuator(*goal_1_led, Actuator::State::OFF), + new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_2"), SensorMeasurement(true)), + new TaskStepActuator(*goal_2_led, Actuator::State::OFF), new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_3"), SensorMeasurement(true)), new TaskStepActuator(*goal_3_led, Actuator::State::OFF), new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_4"), SensorMeasurement(true)), new TaskStepActuator(*goal_4_led, Actuator::State::OFF), - new TaskStepEqual(*get_sensor_by_name("BALL_GOAL_2"), SensorMeasurement(true)), - new TaskStepActuator(*goal_2_led, Actuator::State::OFF), + }; default_task_ = new SequentialTask(*main_steps, "Default Task"); From b5c052dae33d99bdd46f9384b6adf81ef74a37f9 Mon Sep 17 00:00:00 2001 From: peterso Date: Sat, 3 May 2025 21:09:44 +0000 Subject: [PATCH 2/2] clean up comments and add solenoid testing with Core2 BtnC --- .../hal/board/TaskBoardDriver_TBv2025.hpp | 10 +--- idf/taskboard/main/main_core0.cpp | 50 +++++++++++++++++++ 2 files changed, 51 insertions(+), 9 deletions(-) diff --git a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp index 27048e8..fe1c6de 100644 --- a/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp +++ b/idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp @@ -275,15 +275,7 @@ 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); // was here when I started - // gpio_set_level(GPIO_NUM_27, 1); - gpio_set_level(GPIO_NUM_27, state); - - // if (state == Actuator::State::ON){ - // gpio_set_level(GPIO_NUM_27, 1); - // } else { - // gpio_set_level(GPIO_NUM_27, 0); - // } + gpio_set_level(GPIO_NUM_27, state == Actuator::State::ON); }); Actuator* all_goal_leds = new Actuator("ALL_GOAL_LEDS", [&](Actuator::State state) { diff --git a/idf/taskboard/main/main_core0.cpp b/idf/taskboard/main/main_core0.cpp index e3c30cf..37ce607 100644 --- a/idf/taskboard/main/main_core0.cpp +++ b/idf/taskboard/main/main_core0.cpp @@ -51,6 +51,30 @@ void websockets_task( extern "C" void app_main( void) { + // Cycle the solenoid at start up + esp_err_t ret_output; + ret_output = gpio_reset_pin(GPIO_NUM_19); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to reset pin 19"); + return; + } + ret_output = gpio_set_direction(GPIO_NUM_19, GPIO_MODE_OUTPUT); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to set pin 19 direction"); + return; + } + ret_output = gpio_set_level(GPIO_NUM_19, 1); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to set pin 19 level"); + return; + } + vTaskDelay(pdMS_TO_TICKS(1000)); + ret_output = gpio_set_level(GPIO_NUM_19, 0); + if (ret_output != ESP_OK) { + ESP_LOGE("GPIO", "Failed to set pin 19 level"); + return; + } + // ------------------------ // System Initialization // ------------------------ @@ -101,6 +125,10 @@ extern "C" void app_main( screen_controller.clear(); screen_controller.print("-> TaskExecutor started"); + screen_controller.turn_on_gate_LED_clue(); + vTaskDelay(pdMS_TO_TICKS(1000)); + screen_controller.turn_off_gate_LED_clue(); + // Initialize micro-ROS for ROS communication MicroROSController micro_ros_controller; @@ -117,6 +145,7 @@ extern "C" void app_main( // Get references to buttons const SensorReader& BUTTON_A = *task_board_driver.get_sensor_by_name("ON_BOARD_BUTTON_A"); const SensorReader& BUTTON_B = *task_board_driver.get_sensor_by_name("ON_BOARD_BUTTON_B"); + const SensorReader& BUTTON_C = *task_board_driver.get_sensor_by_name("ON_BOARD_BUTTON_C"); const SensorReader& BUTTON_PWR = *task_board_driver.get_sensor_by_name("ON_BOARD_BUTTON_PWR"); const SensorReader& RED_BUTTON = nullptr != task_board_driver.get_sensor_by_name("RED_BUTTON") ? *task_board_driver.get_sensor_by_name("RED_BUTTON") : @@ -343,6 +372,7 @@ extern "C" void app_main( { ESP_LOGI("app_main", "Button A pressed, cancelling current task"); task_executor.cancel_task(); + gpio_set_level(GPIO_NUM_19, 0); // force off the solenoid // Update screen with cancellation message screen_controller.clear(); @@ -368,6 +398,26 @@ extern "C" void app_main( task_board_driver.update(); } } + // Handle Button C press - Toggle solenoid + else if (BUTTON_C.read() == true) + { + ESP_LOGI("app_main", "Button C pressed, toggle solenoid"); + if (!gpio_get_level(GPIO_NUM_19)) + { + gpio_set_level(GPIO_NUM_19, 1); + // TAKE CARE THAT THE SOLENOID IS NOT LEFT ON LONGER THAN 5 SECONDS OR IT GETS VERY HOT + // TODO: ADD TIMER TO TURN SOLENOID OFF AFTER TURNING IT ON. + } else { + gpio_set_level(GPIO_NUM_19, 0); + } + + // Button debounce delay + while (BUTTON_C.read() == true) + { + vTaskDelay(pdMS_TO_TICKS(10)); + task_board_driver.update(); + } + } // If notifed by Micro-ROS task, cancel current task uint32_t value;