Added code for pwm and pub/sub

Author: liamgilligan

.vscode/settings.json

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

src/seahawk/CMakeLists.txt

@@ -17,6 +17,8 @@ add_executable(seahawk
 target_link_libraries(seahawk
     pico_stdlib
     microros
+    hardware_pwm
+    hardware_pio
 )
 
 target_include_directories(seahawk PUBLIC

src/seahawk/seahawk.c

@@ -9,20 +9,50 @@
 
 #include "pico/stdlib.h"
 #include "pico_uart_transports.h"
+#include "hardware/pio.h"
+#include "hardware/pwm.h"
+
 
 const uint LED_PIN = 25;
 
 rcl_publisher_t publisher;
+rcl_subscription_t subscriber;
 std_msgs__msg__Int32 msg;
 
-void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
+// configures the pwm generators
+void config_pwm(uint gpio[], int num_motors, 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);
+    }
+}
+// Sets all of the motors to the given levels
+void sub_callback(uint gpio[], int num_motors, uint16_t levels[]) {    
+    for (int i = 0; i < num_motors; i++) {
+        pwm_set_gpio_level(gpio[i], levels[i]);
+    }
+}
+
+
+void subscription_callback(const void * msgin)
 {
-    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
-    msg.data++;
+  // Process message
+  rcl_ret_t ret = rcl_publish(&publisher, (const std_msgs__msg__Int32 *)msgin, NULL);
 }
 
 int main()
 {
+    const rosidl_message_type_support_t * type_support =
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);
+
     rmw_uros_set_custom_transport(
 		true,
 		NULL,
@@ -58,20 +88,26 @@ int main()
     rclc_support_init(&support, 0, NULL, &allocator);
 
     rclc_node_init_default(&node, "pico_node", "", &support);
+    
+    rclc_subscription_init_default(
+        &subscriber, 
+        &node,
+        type_support, 
+        "pico_in");
+
     rclc_publisher_init_default(
         &publisher,
         &node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
-        "pico_publisher");
-
-    rclc_timer_init_default(
-        &timer,
-        &support,
-        RCL_MS_TO_NS(1000),
-        timer_callback);
+        type_support,
+        "pico_out");
 
     rclc_executor_init(&executor, &support.context, 1, &allocator);
-    rclc_executor_add_timer(&executor, &timer);
+    rclc_executor_add_subscription(
+        &executor, 
+        &subscriber, 
+        &msg,
+        &subscription_callback, 
+        ON_NEW_DATA);
 
     gpio_put(LED_PIN, 1);