wrote microros code for thurster subscriber - to test

Author: JPGC04

pio_workspace/lib/power_custom/power_micro_ros.cpp

@@ -0,0 +1,59 @@
+#include "power_micro_ros.h"
+
+void micro_ros_init() {
+    set_microros_transports();
+
+    allocator = rcl_get_default_allocator();
+
+    // Create init_options
+    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+    // Create node
+    RCCHECK(rclc_node_init_default(&node, "power_node", "", &support));
+
+    // Create subscriber
+    RCCHECK(rclc_subscription_init_default(
+        &propulsion_microseconds_subscriber,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray),
+        "/propulsion/microseconds"));
+
+    propulsion_microseconds_msg.data.data = (int16_t *)malloc(8 * sizeof(int16_t));
+    propulsion_microseconds_msg.data.size = 8;
+    propulsion_microseconds_msg.data.capacity = 8;
+
+    // Create executor
+    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
+    RCCHECK(rclc_executor_add_subscription(&executor, &propulsion_microseconds_subscriber, &propulsion_microseconds_msg, &propulsion_microseconds_subscription_callback, ON_NEW_DATA));
+}
+
+void spin_micro_ros() {
+    RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
+}
+
+void error_loop() {
+  // Ensure msg.data.data is allocated before freeing
+  if (propulsion_microseconds_msg.data.data != NULL) {
+    free(propulsion_microseconds_msg.data.data);
+  }
+
+  int error = 0;
+  
+  while(error <= 20) {
+    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
+    delay(100);
+
+    error += 1;
+  }
+
+  digitalWrite(LED_PIN, HIGH);
+}
+
+void propulsion_microseconds_subscription_callback(const void * msgin) {
+    const std_msgs__msg__Int16MultiArray * propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
+
+    // Ensure we don't exceed the size of the `microseconds` array
+    for (size_t i = 0; i < 8 && i < propulsion_microseconds_msg->data.size; i++) {
+        microseconds[i] = propulsion_microseconds_msg->data.data[i]; // Access the data correctly
+    }
+}

pio_workspace/lib/power_custom/power_micro_ros.h

@@ -0,0 +1,31 @@
+#include <micro_ros_arduino.h>
+
+#include <Arduino.h>
+
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+#include <std_msgs/msg/int16_multi_array.h>
+
+#define LED_PIN 13
+
+rcl_subscription_t propulsion_microseconds_subscriber;
+std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
+rclc_executor_t executor;
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+rcl_timer_t timer;
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
+
+extern int16_t microseconds[8];
+
+void micro_ros_init();
+void spin_micro_ros();
+void error_loop();
+
+void propulsion_microseconds_subscription_callback(const void * msgin);

pio_workspace/src/power_main.cpp

@@ -3,80 +3,25 @@
 #include "TMP36.h"
 #include "adc_sensors.h"
 #include "ThrusterControl.h"
+#include "power_micro_ros.h"
 
-#define LED 13
-#define TEMP_SENSE 23
-#define THRUSTER_DELAY 1000  // Delay time in milliseconds
-
-ADCSensors ADCs;
-TMP36 TEMPSENSOR(TEMP_SENSE, 3.3);
-
-// A variable to track the current thruster being updated
-int currentThruster = 0;
+#define LED_PIN 13
 
 void power_setup() {
-    // an interrupt would generally be setup for water detection
+    pinMode(LED_PIN, OUTPUT);
+    digitalWrite(LED_PIN, HIGH);
 
-    pinMode(LED, OUTPUT);
-    digitalWrite(LED, HIGH);
     initThrusters();
 
-    Serial.begin(115200);
-    Serial.println("Initializing ADCs...");
-    if (!ADCs.begin(true, true, &Wire1)) {
-        Serial.println("Failed to init ADCs");
-    }
-
-    Serial.println("Initializing Temperature Sensor");
-    TEMPSENSOR.begin();
+    micro_ros_init();
+  
+    delay(2000);
 }
 
 void power_loop() {
-    // Reset the microseconds array to default value (1500) for all thrusters
-    for (int i = 0; i < 8; i++) {
-        microseconds[i] = 1500;
-    }
-
-    // Update the current thruster
-    microseconds[currentThruster] = 1560;
     updateThrusters(microseconds);
 
-    // Increment to the next thruster for the next loop
-    currentThruster = (currentThruster + 1) % 8;
-
-    // Print the voltages, currents, and temperature
-    float* voltages = ADCs.senseVoltage();
-    if (voltages) {
-        Serial.print("Voltage 1: ");
-        Serial.println(voltages[0]);
-        Serial.print("Voltage 2: ");
-        Serial.println(voltages[1]);
-    } else {
-        Serial.println("Failed to read voltages");
-    }
-
-    // Print the currents
-    float* currents = ADCs.senseCurrent();
-    if (currents) {
-        for (int i = 0; i < 8; i++) {
-            Serial.print("Current ");
-            Serial.print(i + 1);
-            Serial.print(": ");
-            Serial.println(currents[i]);
-        }
-    } else {
-        Serial.println("Failed to read currents");
-    }
-
-    // Print the temperature
-    float temperature = TEMPSENSOR.readTemperature();
-    Serial.print("Temperature: ");
-    Serial.println(temperature);
-
-    // Delay after updating one thruster
-    delay(THRUSTER_DELAY);
-
-    // Reset the current thruster to the off state (1500)
-    microseconds[currentThruster] = 1500;
-    updateThrusters(microseconds);
-}
+    spin_micro_ros();
+    
+    delay(10);
+}