Servo continuous persistence

.vscode/settings.json

@@ -5,11 +5,13 @@
         "${workspaceFolder}/talos/PoacBoard/",
         "${workspaceFolder}/talos/SmartBattery/",
         "${workspaceFolder}/talos/LEDBoard/",
+        "${workspaceFolder}/talos/micro_ros_template/",
         "${workspaceFolder}/talos/IVCBoard/",
         "${workspaceFolder}/puddles/Backplane/",
         "${workspaceFolder}/tools/upload_tool/",
         "${workspaceFolder}/tools/canmore_cli/",
         "${workspaceFolder}/SBCBoard",
+        ,
         "${workspaceFolder}/lilboat/HBridgeBoard/",
         "${workspaceFolder}/lilboat/PoHalfBoard/"
     ],

lilboat/HBridgeBoard/CMakeLists.txt

@@ -43,24 +43,27 @@ pico_generate_pio_header(hbridge_board ${CMAKE_CURRENT_LIST_DIR}/src/actuators/u
 
 # Enable the 'make upload' and 'make info' commands for project
 titan_use_upload_tool(hbridge_board)
-titan_enable_bootloader(hbridge_board can)  # TODO: Select the same transport as below, or remove this line if using USB
+#titan_enable_bootloader(hbridge_board can)  # TODO: Select the same transport as below, or remove this line if using USB
 
 # Define linking and targets
 pico_add_extra_outputs(hbridge_board)
 target_link_libraries(hbridge_board PUBLIC
     pico_stdlib     # Pico SDK Standard Library
     driver_led      # Status LED support
     # TODO: Select your transport:
-    micro_ros_can   # Micro ros support (supports _can, _usb, _eth)
+    micro_ros_usb   # Micro ros support (supports _can, _usb, _eth)
     titan_logger    # A nice little logging library
     titan_safety    # Handles critical code (Watchdog, Kill Switch, Fault Reporting)
+    titan_debug
 
     # TODO: Add any additional hardware here (ex. hardware_adc)
     hardware_pwm
     hardware_dma
     hardware_pio
     hardware_irq
     hardware_adc
+    hardware_flash
+    hardware_sync
     driver_async_i2c
     titan_queue
     driver_cd74hc4051

lilboat/HBridgeBoard/include/actuators/actuator.h

@@ -5,9 +5,27 @@
 
 #include "pico/stdlib.h"
 
+#define NUM_SERVOS 2
+
 #define SERVO_MIN_DEG 0
 #define SERVO_MAX_DEG 240
 
+#define UNITS_PER_DEGREE 65536.0f / 360.0f
+
+#define FLASH_SIZE_BYTES 2097152
+#define FLASH_SECTOR_BYTES 4096
+#define FLASH_OFFSET FLASH_SIZE_BYTES - FLASH_SECTOR_BYTES
+#define PAGE_SIZE_BYTES 256
+#define XIP_BASE_ADDRESS 0x10000000
+#define ROBOT_MAGIC_NUMBER 1
+
+typedef struct {
+    uint32_t magic_number;
+    int32_t abs_position[NUM_SERVOS];
+} config_t;
+
+extern config_t servo_config;
+
 extern void servo_ping(servo_t *servo);
 
 extern void servo_set_armed(servo_t *servo, bool armed);
@@ -22,6 +40,12 @@ extern void servo_continuous_move_ms(servo_t *servo, int16_t speed, uint32_t ms)
 
 extern void servo_continuous_set_deg(servo_t *servo, int16_t speed, float deg);
 
+extern void servo_continuous_move_deg(servo_t *servo, int32_t target_deg, int16_t speed);
+
+extern void simuilate_encoder(servo_t *servo);
+
+extern void servo_read_continuous(servo_t *servo);
+
 extern void servo_set_id(uint8_t old_id, uint8_t new_id);
 
 extern void servo_read_id();
@@ -32,6 +56,10 @@ extern void servo_set_home(servo_t *servo);
 
 extern void servo_init_internal();
 
-extern void make_servo(servo_t *servo, uint8_t id, uint16_t home_deg);
+extern void make_servo(servo_t *servo, uint8_t id, uint16_t home_deg, int32_t abs_position);
+
+extern void write_to_flash(config_t *config);
+
+extern bool read_from_flash(config_t *data);
 
 #endif  // ACTUATOR_H

lilboat/HBridgeBoard/include/actuators/hiwonder_driver.h

@@ -113,6 +113,10 @@ typedef struct servo {
     uint16_t home_deg;
     uint16_t curr_deg;
 
+    bool is_moving;
+    int32_t target_pos_continuous;
+    bool target_pos_reached;
+
     // TODO: this is cursed
     bool is_sethome_req;
     bool return_home_after_move;
@@ -132,6 +136,9 @@ typedef struct servo {
     absolute_time_t move_timeout;
     uint16_t curr_move_time_ms;
 
+    int32_t absolute_pos;
+    int16_t last_position;
+
     // Alarms and timers
     alarm_id_t move_complete_alarm;
     alarm_id_t start_go_home_alarm;

lilboat/HBridgeBoard/include/actuators/ros_actuators.h

@@ -1,10 +1,16 @@
 #ifndef ROS_ACTUATORS_H
 #define ROS_ACTUATORS_H
 
+#include "actuator.h"
+#include "actuators/hiwonder_driver.h"
 #include "ros.h"
 
 extern const size_t ros_actuators_num_executor_handles;
 
+extern servo_t claw_grip;
+extern servo_t claw_rack;
+extern servo_t *servos[NUM_SERVOS];
+
 extern void servo_ping_all();
 
 extern rcl_ret_t ros_actuators_update_status();

lilboat/HBridgeBoard/include/ros.h

@@ -92,6 +92,9 @@ rcl_ret_t ros_update_firmware_status(uint8_t client_id);
 
 // TODO: Add in any additional ROS tasks here
 
-#define MICRO_ROS_TRANSPORT_CAN 1
+// #define MICRO_ROS_TRANSPORT_CAN 1
+
+#undef MICRO_ROS_TRANSPORT_CAN
+#define MICRO_ROS_TRANSPORT_USB 1
 
 #endif

lilboat/HBridgeBoard/src/actuators/actuator.c

@@ -3,7 +3,9 @@
 #include "actuators/async_uart.h"
 #include "actuators/hiwonder_driver.h"
 
+#include "hardware/flash.h"
 #include "hardware/pio.h"
+#include "hardware/sync.h"
 #include "titan/debug.h"
 #include "titan/logger.h"
 
@@ -17,6 +19,8 @@
 
 uint8_t discovered_id = 0;
 
+config_t servo_config;
+
 // Handlers
 // dxlact_idle_position_handler_t idle_handler;
 // dxlact_move_done_handler_t done_handler;
@@ -43,7 +47,7 @@ static void servo_ping_cb(ServoPacket_t rx_packet, enum servo_read_err err) {
     else {
         servo->num_errors = 0;
         servo->connected = true;
-        // LOG_INFO("Actuator connected!");
+        LOG_INFO("Actuator connected!");
     }
 }
 
@@ -176,6 +180,7 @@ static void servo_read_deg_cb(ServoPacket_t rx_packet, enum servo_read_err err)
 
     uint16_t pos = rx_packet.param_buf[0] | rx_packet.param_buf[1] << 8;
     pos = pos > 1000 ? 1000 : pos;
+    pos = pos < 0 ? 0 : pos;
     servo->curr_deg = pos * (240.0 / 1000.0);
 
     LOG_INFO("Read %hd pos as %hd deg", pos, servo->curr_deg);
@@ -227,10 +232,105 @@ void servo_continuous_move_ms(servo_t *servo, int16_t speed, uint32_t ms) {
 // For now, just big trust this command goes through
 void servo_continuous_set_deg(servo_t *servo, int16_t speed, float deg) {}
 
+void servo_stop_check(servo_t *servo) {
+    const int32_t STOPPING_TOLERANCE = 10;  // Adjust value
+
+    if (abs(servo->absolute_pos - servo->target_pos_continuous) < STOPPING_TOLERANCE) {
+        // Stop servo
+        uint8_t param_buf[MAX_PACKET_SIZE];  // Is MAX_PACKET_SIZE necessary
+        ServoPacket_t stop_servo_packet = make_servo_packet(servo, SERVO_MOVE_STOP_CMD, SERVO_MOVE_STOP_LEN, param_buf);
+        write_to_flash(&servo_config);
+
+        if (enqueue_packet(stop_servo_packet)) {
+            servo->is_moving = false;
+        }
+    }
+}
+
+void servo_continuous_move_deg(servo_t *servo, int32_t target_deg, int16_t speed) {
+    uint8_t param_buf[MAX_PACKET_SIZE];
+    param_buf[0] = 1;
+    param_buf[2] = (uint8_t) (speed & 0x00FF);
+    param_buf[3] = (uint8_t) (speed >> 8);
+
+    servo->target_pos_continuous = (servo->absolute_pos + (UNITS_PER_DEGREE * target_deg));
+
+    ServoPacket_t write_continuous_deg_packet =
+        make_servo_packet(servo, SERVO_OR_MOTOR_MODE_WRITE_CMD, SERVO_OR_MOTOR_MODE_WRITE_LEN, param_buf);
+
+    if (enqueue_packet(write_continuous_deg_packet)) {
+        servo->is_moving = true;
+    }
+}
+
+void servo_read_continuous_cb(ServoPacket_t rx_packet, enum servo_read_err err) {
+    if (err != SERVO_READ_OK) {
+        return;
+    }
+    servo_t *servo = rx_packet.servo;
+
+    /*
+    if (!servo->is_moving) {
+        return;
+    }
+    */
+
+    if (err != SERVO_READ_OK) {
+        LOG_WARN("Servo read error");
+    }
+
+    const int32_t ROLLOVER_THRESHOLD = 500;  // 32768
+    const int32_t FULL_RANGE = 1400;
+
+    // int16_t last_position = (int16_t) servo->absolute_pos;
+    int16_t last_position = servo->last_position;
+    int16_t curr_position = rx_packet.param_buf[1] << 8 | rx_packet.param_buf[0];
+
+    // printf("Current position: %d", curr_position);
+
+    int32_t position_change = curr_position - last_position;
+
+    if (position_change <= -ROLLOVER_THRESHOLD) {
+        servo->absolute_pos += FULL_RANGE;
+    }
+    else if (position_change >= ROLLOVER_THRESHOLD) {
+        servo->absolute_pos -= FULL_RANGE;
+    }
+
+    /*
+    uint16_t last_position = (uint16_t) servo->absolute_pos;
+    uint16_t curr_position = rx_packet.param_buf[3] << 8 | rx_packet.param_buf[2];
+
+    if (last_position >= HALF_RANGE && curr_position <= HALF_RANGE) {
+        servo->absolute_pos += FULL_RANGE;
+    }
+
+    else if (last_position <= HALF_RANGE && curr_position >= HALF_RANGE) {
+        servo->absolute_pos -= FULL_RANGE
+    }
+    */
+
+    printf("Servo position: %d", servo->absolute_pos);
+
+    servo->absolute_pos += position_change;
+    servo->last_position = curr_position;
+    servo_stop_check(servo);
+}
+
+void servo_read_continuous(servo_t *servo) {
+    uint8_t param_buf[MAX_PACKET_SIZE];
+    //  param_buf[0] = 1;
+    //  param_buf[1] = 0;
+
+    ServoPacket_t read_continuous_packet = make_servo_packet(servo, SERVO_POS_READ_CMD, SERVO_POS_READ_LEN, param_buf);
+    read_continuous_packet.on_read = servo_read_continuous_cb;
+
+    enqueue_packet(read_continuous_packet);
+}
+
 void servo_set_id(uint8_t old_id, uint8_t new_id) {
     uint8_t param_buf[MAX_PACKET_SIZE];
     param_buf[0] = new_id;
-
     servo_t tmp = { .id = old_id };
     ServoPacket_t set_id_packet = make_servo_packet(&tmp, SERVO_ID_WRITE_CMD, SERVO_ID_WRITE_LEN, param_buf);
 
@@ -398,11 +498,33 @@ void servo_init_internal() {
     // servo_go_home();
 }
 
-void make_servo(servo_t *servo, uint8_t id, uint16_t home_deg) {
+void make_servo(servo_t *servo, uint8_t id, uint16_t home_deg, int32_t abs_position) {
     servo->is_sethome_req = false;
     servo->return_home_after_move = false;
     servo->desired_armed_state = false;
+    servo->absolute_pos = abs_position;
 
     servo->id = id;
     servo->home_deg = home_deg;
 }
+
+void write_to_flash(config_t *config) {
+    uint32_t prev_interrupt = save_and_disable_interrupts();
+
+    flash_range_erase(FLASH_OFFSET, FLASH_SECTOR_BYTES);
+
+    uint8_t buffer[FLASH_PAGE_SIZE] = { 0 };
+    config->magic_number = ROBOT_MAGIC_NUMBER;
+    memcpy(buffer, config, sizeof(config_t));
+    printf("Magic number: %d", buffer[0]);
+    flash_range_program(FLASH_OFFSET, buffer, PAGE_SIZE_BYTES);
+
+    restore_interrupts(prev_interrupt);
+}
+
+bool read_from_flash(config_t *data) {
+    const config_t *flash_contents = (config_t *) (XIP_BASE_ADDRESS + FLASH_OFFSET);
+    memcpy(data, flash_contents, sizeof(config_t));
+
+    return data->magic_number == ROBOT_MAGIC_NUMBER;
+}

lilboat/HBridgeBoard/src/actuators/hiwonder_driver.c

@@ -123,9 +123,13 @@ void send_packet(ServoPacket_t packet) {
     raw_packet[4] = packet.command;
 
     // TODO: replace with memcpy?
-    for (uint8_t i = 0; i < 7 - 3; i++) {
+    /*
+    for (uint8_t i = 0; i < packet.command_length - 3; i++) {
         raw_packet[i + 5] = packet.param_buf[i];
     }
+    */
+
+    memcpy(&raw_packet[5], packet.param_buf, packet.command_length - 3);
 
     raw_packet[packet_size - 1] = calculate_checksum(&packet);
 
@@ -134,8 +138,9 @@ void send_packet(ServoPacket_t packet) {
 }
 
 bool enqueue_packet(ServoPacket_t packet) {
-    if (QUEUE_FULL(&tx_queue))
+    if (QUEUE_FULL(&tx_queue)) {
         return false;
+    }
 
     ServoPacket_t *entry = QUEUE_CUR_WRITE_ENTRY(&tx_queue);
     // This copy is generally safe since there are no (non-function) pointers in ServoPacket