Basic PWM implementation

.vscode/settings.json

@@ -5,4 +5,12 @@
     },
     "git.detectSubmodules": false,
     "cmake.sourceDirectory": "${workspaceFolder}/src/seahawk",
+    "files.associations": {
+        "stdio.h": "c",
+        "rmw_microros.h": "c",
+        "int32.h": "c",
+        "int16_multi_array.h": "c",
+        "pico_uart_transports.h": "c",
+        "stdint.h": "c"
+    },
 }

src/seahawk/CMakeLists.txt

@@ -11,12 +11,16 @@ pico_sdk_init()
 link_directories($ENV{MICRO_ROS_RPI_PICO_PATH}/libmicroros)
 
 add_executable(seahawk
+    crc.c
+    uart_proto.c
     seahawk.c
     pico_uart_transport.c
 )
 target_link_libraries(seahawk
     pico_stdlib
     microros
+    hardware_pwm
+    hardware_pio
 )
 
 target_include_directories(seahawk PUBLIC

src/seahawk/crc.c

@@ -0,0 +1,44 @@
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "crc.h"
+
+uint8_t get_crc_byte(const uint8_t *buffer, uint16_t length) {
+    uint8_t retVal = 0u;
+    uint16_t byteIndex = 0u;
+
+    if (buffer != NULL) {
+        uint8_t bitIndex = 0u;
+        retVal = CRC_8_INIT_VALUE;
+
+        // Do calculation procedure for each byte
+        for (byteIndex = 0u; byteIndex < length; byteIndex++) {
+            // XOR new byte with temp result
+            retVal ^= (buffer[byteIndex] << (CRC_8_RESULT_WIDTH - 8u));
+            // Do calculation for current data
+            for (bitIndex = 0u; bitIndex < 8u; bitIndex++) {
+                if (retVal & (1u << (CRC_8_RESULT_WIDTH - 1u))) {
+                    retVal = (retVal << 1u) ^ CRC_8_POLYNOMIAL;
+                }
+                else {
+                    retVal = (retVal << 1u);
+                }
+            }
+        }
+        // XOR result with specified value
+        retVal ^= CRC_8_XOR_VALUE;
+    }
+    return retVal;
+}
+
+void add_crc_byte(uint8_t *buffer, uint16_t length) {
+	uint8_t crc = get_crc_byte(buffer, length);
+	// Increase buffer size by 1
+	buffer = (uint8_t *) realloc(buffer, length + 1);
+	buffer[length] = crc;
+}
+
+bool check_crc(const uint8_t *buffer, uint16_t length_no_crc) {
+	return get_crc_byte(buffer, length_no_crc) == buffer[length_no_crc];
+}

src/seahawk/crc.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdbool.h>
+
+// Constants used in CRC generation
+
+#define CRC_8_RESULT_WIDTH                  8u
+#define CRC_8_POLYNOMIAL                    0x1Du
+#define CRC_8_INIT_VALUE                    0xFFu
+#define CRC_8_XOR_VALUE                     0xFFu
+
+// Given a buffer, generate a CRC byte.
+uint8_t get_crc_byte(const uint8_t *buffer, uint16_t length);
+
+// Given a buffer, append a CRC byte.
+void add_crc_byte(uint8_t *buffer, uint16_t length);
+
+// Length should be of buffer without the crc
+bool check_crc(const uint8_t *buffer, uint16_t length_no_crc);

src/seahawk/pico_uart_transport.c

@@ -2,6 +2,7 @@
 #include "pico/stdlib.h"
 
 #include <uxr/client/profile/transport/custom/custom_transport.h>
+#include <hardware/uart.h>
 
 void usleep(uint64_t us)
 {
@@ -19,6 +20,11 @@ int clock_gettime(clockid_t unused, struct timespec *tp)
 bool pico_serial_transport_open(struct uxrCustomTransport * transport)
 {
     stdio_init_all();
+    uart_init(uart0, 115200);
+    gpio_set_function(0, GPIO_FUNC_UART);
+    gpio_set_function(1, GPIO_FUNC_UART);
+    uart_set_hw_flow(uart0, false, false);
+    uart_set_format(uart0, 8, 1, UART_PARITY_NONE);
     return true;
 }
 

src/seahawk/seahawk.c

@@ -5,24 +5,99 @@
 #include <rclc/rclc.h>
 #include <rclc/executor.h>
 #include <std_msgs/msg/int32.h>
+#include <std_msgs/msg/int16_multi_array.h>
+#include <std_msgs/msg/float64_multi_array.h>
 #include <rmw_microros/rmw_microros.h>
 
 #include "pico/stdlib.h"
 #include "pico_uart_transports.h"
+#include "hardware/pio.h"
+#include "hardware/pwm.h"
+
+#define BMS 0
+#include "uart_proto.h"
+#include "crc.h"
+
+const uart_inst inst = {uart0, 0, 1, 115200};
 
 const uint LED_PIN = 25;
 
 rcl_publisher_t publisher;
-std_msgs__msg__Int32 msg;
+rcl_subscription_t subscriber;
+const uint GPIO[8] = {20, 13, 19, 11, 21, 12, 18, 10};
+const int NUM_MOTORS = 8;
+const uint8_t CLOCK_DIV = 125;
+
+// configures the pwm generators
+void config_pwm(uint16_t wrap) {
+    for (int i = 0; i < NUM_MOTORS; i++) {
+      // Sets all GPIO pins to use PWM
+      gpio_set_function(GPIO[i], GPIO_FUNC_PWM);
+      // Sets the wrap for each slice
+      pwm_set_wrap(pwm_gpio_to_slice_num(GPIO[i]), wrap);
+      /*
+       * If phase correct is set to false the counter
+       * will reset to 0 after reaching the level,
+       * otherwise it will decrease down to 0
+       */
+      pwm_set_phase_correct(pwm_gpio_to_slice_num(GPIO[i]), false);
+      // Set clock division
+      pwm_set_clkdiv_int_frac(pwm_gpio_to_slice_num(GPIO[i]), CLOCK_DIV, 0);
+      // Set pins to 1500microseconds for neutral
+      pwm_set_gpio_level(GPIO[i], 1500);
+      // Enable PWM
+      pwm_set_enabled(pwm_gpio_to_slice_num(GPIO[i]), true);
+    }
+}
+// Sets all of the motors to the given levels
+void set_duty_cycle(uint16_t levels[]) {    
+    for (int i = 0; i < NUM_MOTORS; i++) {
+        pwm_set_gpio_level(GPIO[i], levels[i]);
+    }
+}
+
+int mothertrucker = 0;
+
+void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
+    sensor_data retval;
+    send_request(inst);
+    get_response(inst, &retval);
+    std_msgs__msg__Float64MultiArray msg;
+    double float_data[9] = {
+        (double) retval.kill,
+        (double) retval.ina780_current,
+        (double) retval.ina780_voltage,
+        (double) retval.ina780_temperature,
+        (double) retval.ina780_power,
+        (double) retval.ina780_energy,
+        (double) retval.bme280_temperature,
+        (double) retval.bme280_hum,
+        (double) retval.bme280_press,
+    };
+    msg.data.data = float_data;
+    msg.data.size = 9;
+    msg.data.capacity = 9;
+    msg.layout.dim.size = 0;
+    msg.layout.dim.capacity = 0;
+    msg.layout.dim.data = NULL;
+    msg.layout.data_offset = 0;
+    mothertrucker = (mothertrucker + 1) % 2;
+    gpio_put(LED_PIN, mothertrucker);
+    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL); // ignore
+}
 
-void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
+void subscription_callback(const void * msgin)
 {
-    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
-    msg.data++;
+    // Set the msgin to a Int16MultiArray
+    std_msgs__msg__Int16MultiArray *msg = (std_msgs__msg__Int16MultiArray *) msgin;
+    set_duty_cycle((uint16_t *) msg->data.data);
 }
 
 int main()
 {
+    // const rosidl_message_type_support_t * type_support =
+    //     ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray);
+
     rmw_uros_set_custom_transport(
 		true,
 		NULL,
@@ -35,7 +110,6 @@ int main()
     gpio_init(LED_PIN);
     gpio_set_dir(LED_PIN, GPIO_OUT);
 
-    rcl_timer_t timer;
     rcl_node_t node;
     rcl_allocator_t allocator;
     rclc_support_t support;
@@ -55,27 +129,96 @@ int main()
         return ret;
     }
 
-    rclc_support_init(&support, 0, NULL, &allocator);
+    config_pwm(3000);
 
+    rclc_support_init(&support, 0, NULL, &allocator);
+
     rclc_node_init_default(&node, "pico_node", "", &support);
-    rclc_publisher_init_default(
-        &publisher,
+
+    // Define pwm_msg
+    std_msgs__msg__Int16MultiArray pwm_msg;
+
+    // Define msg data Int16 Sequence
+    rosidl_runtime_c__int16__Sequence pwm_msg_data;
+    pwm_msg_data.size = 0;
+    pwm_msg_data.capacity = 8;
+    int16_t pwm_msg_data_data[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+    pwm_msg_data.data = pwm_msg_data_data;
+
+    pwm_msg.data = pwm_msg_data;
+
+    // Define msg layout MultiArray Layout
+
+    std_msgs__msg__MultiArrayLayout pwm_msg_layout;
+
+    std_msgs__msg__MultiArrayDimension__Sequence pwm_msg_layout_dim;
+    pwm_msg_layout_dim.size = 0;
+    pwm_msg_layout_dim.capacity = 0;
+    pwm_msg_layout_dim.data = NULL;
+
+    pwm_msg_layout.dim = pwm_msg_layout_dim;
+    pwm_msg_layout.data_offset = 0;
+
+    pwm_msg.layout = pwm_msg_layout;
+
+    // Define bms_msg
+    std_msgs__msg__Float64MultiArray bms_msg;
+
+    // Define bms_msg data Float64 Sequence
+    rosidl_runtime_c__double__Sequence bms_msg_data;
+    bms_msg_data.size = 0;
+    bms_msg_data.capacity = 9;
+    double bms_msg_data_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+    bms_msg_data.data = bms_msg_data_data;
+
+    bms_msg.data = bms_msg_data;
+
+    // Define bms_msg_layout
+
+    std_msgs__msg__MultiArrayLayout bms_msg_layout;
+
+    std_msgs__msg__MultiArrayDimension__Sequence bms_msg_layout_dim;
+    bms_msg_layout_dim.size = 0;
+    bms_msg_layout_dim.capacity = 0;
+    bms_msg_layout_dim.data = NULL;
+
+    bms_msg_layout.dim = bms_msg_layout_dim;
+    bms_msg_layout.data_offset = 0;
+
+    bms_msg.layout = bms_msg_layout;
+
+    uart_initialization(inst);
+
+    const unsigned int timer_period = RCL_MS_TO_NS(1000);
+    rcl_timer_t timer;
+    rcl_ret_t rc = rclc_timer_init_default(&timer, &support, timer_period, timer_callback);
+
+
+    ret = rclc_subscription_init_default(
+        &subscriber, 
         &node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
-        "pico_publisher");
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray), 
+        "pwm_values");
 
-    rclc_timer_init_default(
-        &timer,
-        &support,
-        RCL_MS_TO_NS(1000),
-        timer_callback);
+    ret = rclc_publisher_init_default(
+        &publisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
+        "bms_data");
 
     rclc_executor_init(&executor, &support.context, 1, &allocator);
-    rclc_executor_add_timer(&executor, &timer);
+    rclc_executor_add_subscription(
+        &executor, 
+        &subscriber, 
+        &pwm_msg,
+        &subscription_callback, 
+        ON_NEW_DATA);
+
+    rc = rclc_executor_add_timer(&executor, &timer);
 
     gpio_put(LED_PIN, 1);
 
-    msg.data = 0;
+
     while (true)
     {
         rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));