Revamped Service Light, much IMU headway.

examples/9DOF_IMU_Robot/9DOF_IMU_Robot.ino

@@ -2,8 +2,7 @@
 
 void setup() {
   Serial.begin(115200);
-  NoU3.beginMotors();
-  NoU3.beginIMUs();
+  NoU3.begin();
 
   Serial.println("Accel_X\tAccel_Y\tAccel_Z\tGyro_X\tGyro_Y\tGyro_Z\tMag_X\tMag_Y\tMag_Z");
 }

examples/MotorParty/MotorParty.ino

@@ -19,15 +19,38 @@ NoU_Servo servo2(2);
 NoU_Servo servo3(3);
 NoU_Servo servo4(4);
 
+//FVT mode is used for "Functional Verification testing"
+//This is used internally by Alfredo to test all NoU3s before they pass QA testing
+
+//#define FVT_MODE
+
+#ifdef FVT_MODE
+bool startTest = false;
+float motorPeriod = 0.05;
+#else
+bool startTest = true;
+float motorPeriod = 0.2;
+#endif
+
 void setup() {
     NoU3.begin();
     Serial.begin(115200);
-    RSL::initialize();
-    RSL::setState(RSL_ENABLED);
+
+    pinMode(0, INPUT_PULLUP);
+    delay(1000);
 }
 
 void loop() {
-    for (float i = -1; i < 1; i += 0.05) {
+  NoU3.updateServiceLight();
+
+  if(digitalRead(0) != 1){
+    startTest = true;
+  }
+
+  if(startTest){
+    NoU3.setServiceLight(LIGHT_ENABLED);
+
+    for (float i = -1; i < 1; i += motorPeriod) {
         motor1.set(i);
         motor2.set(i);
         motor3.set(i);
@@ -41,12 +64,11 @@ void loop() {
         servo2.write(map(i*100.0, -100, 100, 0, 180));
         servo3.write(map(i*100.0, -100, 100, 0, 180));
         servo4.write(map(i*100.0, -100, 100, 0, 180));
-
-        RSL::update();
+
         delay(5);
         Serial.println(i);
     }
-    for (float i = 1; i > -1; i -= 0.05) {
+    for (float i = 1; i > -1; i -= motorPeriod) {
         motor1.set(i);
         motor2.set(i);
         motor3.set(i);
@@ -61,8 +83,8 @@ void loop() {
         servo3.write(map(i*100.0, -100, 100, 0, 180));
         servo4.write(map(i*100.0, -100, 100, 0, 180));
 
-        RSL::update();
         delay(5);
         Serial.println(i);
     }
+  }
 }

examples/PestoLink_ArcadeBot/PestoLink_ArcadeBot.ino

@@ -18,20 +18,16 @@ NoU_Servo servo(1);
 NoU_Drivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &rearLeftMotor, &rearRightMotor);
 
 void setup() {
-    //EVERYONE SHOULD CHANGE "ESP32 Bluetooth" TO THE NAME OF THEIR ROBOT HERE BEFORE PAIRING THEIR ROBOT TO ANY LAPTOP
+    //EVERYONE SHOULD CHANGE "NoU3_Bluetooth" TO THE NAME OF THEIR ROBOT HERE BEFORE PAIRING THEIR ROBOT TO ANY LAPTOP
     NoU3.begin();
-    PestoLink.begin("ESP32_Bluetooth");
+    PestoLink.begin("NoU3_Bluetooth");
     Serial.begin(115200);
 
     // If a motor in your drivetrain is spinning the wrong way, change the value for it here from 'false' to 'true'
     frontLeftMotor.setInverted(false);
     frontRightMotor.setInverted(true);
     rearLeftMotor.setInverted(false);
     rearRightMotor.setInverted(true);
-
-    // Important for your Robot Service Light, no need to mess with this code
-    RSL::initialize();
-    RSL::setState(RSL_ENABLED);
 }
 
 void loop() {
@@ -48,9 +44,9 @@ void loop() {
 
         drivetrain.arcadeDrive(throttle, rotation);
 
-        RSL::setState(RSL_ENABLED);
+        NoU3.setServiceLight(LIGHT_ENABLED);
     } else {
-        RSL::setState(RSL_DISABLED);
+        NoU3.setServiceLight(LIGHT_DISABLED);
     }
 
     // Here we decide what the servo angle will be based on if button 0 is pressed
@@ -68,5 +64,5 @@ void loop() {
 
     // No need to mess with this code
     PestoLink.update();
-    RSL::update();
+    NoU3.updateServiceLight();
 }

examples/sensor_fusion/sensor_calibration_CALfredo/sensor_calibration_CALfredo.ino

@@ -0,0 +1,41 @@
+#include <Alfredo_NoU3.h>
+
+void setup() {
+  Serial.begin(115200);
+  NoU3.begin();
+}
+
+long lastPrintMs = 0;  // Stores the last time the function was called
+
+void loop() {
+
+  NoU3.updateIMUs();
+
+  if (millis() - lastPrintMs >= 20) {
+    lastPrintMs = millis();  // Update the last time the function was called
+    formatPrint(NoU3.acceleration_x);
+    Serial.print(", ");
+    formatPrint(NoU3.acceleration_y);
+    Serial.print(", ");
+    formatPrint(NoU3.acceleration_z);
+    Serial.print(", ");
+    formatPrint(NoU3.gyroscope_x);
+    Serial.print(", ");
+    formatPrint(NoU3.gyroscope_y);
+    Serial.print(", ");
+    formatPrint(NoU3.gyroscope_z);
+    Serial.print(", ");
+    formatPrint(NoU3.magnetometer_x);
+    Serial.print(", ");
+    formatPrint(NoU3.magnetometer_y);
+    Serial.print(", ");
+    formatPrint(NoU3.magnetometer_z);
+    Serial.println("");
+  }
+}
+
+void formatPrint(float num) {
+  char buffer[11];
+  snprintf(buffer, sizeof(buffer), "%.3f ", num);
+  Serial.print(buffer);
+}

examples/sensor_fusion/sensor_calibration_measure_accel_and_gyro/sensor_calibration_measure_accel_and_gyro.ino → examples/sensor_fusion/sensor_calibration_measure_accel_and_gyro_offset/sensor_calibration_measure_accel_and_gyro_offset.ino

@@ -1,6 +1,7 @@
 #include <Alfredo_NoU3.h>
 
 const int numMeasurements = 2000;
+
 float acceleration_x[numMeasurements];
 float acceleration_y[numMeasurements];
 float acceleration_z[numMeasurements];
@@ -17,17 +18,16 @@ void setup() {
     ; // Wait here until the Serial Monitor is opened
   }
 
-  NoU3.beginMotors();
-  NoU3.beginIMUs();
+  NoU3.begin();
 
   Serial.println("Serial connection established. Starting data collection...");
 }
 
 void loop() {
-  NoU3.updateIMUs();
 
   // Check if data is available and measurements are not complete
-  if (NoU3.checkDataIMU() && !measurementsComplete) {
+  if (!measurementsComplete && NoU3.updateIMUs()) {
+
     // Store measurements in arrays
     acceleration_x[currentIndex] = NoU3.acceleration_x;
     acceleration_y[currentIndex] = NoU3.acceleration_y;
@@ -40,6 +40,8 @@ void loop() {
 
     // Check if we've reached the target number of measurements
     if (currentIndex >= numMeasurements) {
+      Serial.print(currentIndex);
+      Serial.println(" measurements taken");
       measurementsComplete = true;
       calculateAndPrintAverages();
     }
@@ -69,11 +71,11 @@ void calculateAndPrintAverages() {
   float avgGz = sumGz / numMeasurements;
 
   // Print averages
-  Serial.println("Average values after 2000 measurements:");
-  Serial.print("Acceleration X: "); Serial.println(avgAx);
-  Serial.print("Acceleration Y: "); Serial.println(avgAy);
-  Serial.print("Acceleration Z: "); Serial.println(avgAz);
-  Serial.print("Gyroscope X: "); Serial.println(avgGx);
-  Serial.print("Gyroscope Y: "); Serial.println(avgGy);
-  Serial.print("Gyroscope Z: "); Serial.println(avgGz);
+  Serial.print("Average values after "); Serial.print(currentIndex); Serial.println(" measurements:");
+  Serial.print("Acceleration X (Gs): "); Serial.println(avgAx, 3);
+  Serial.print("Acceleration Y (Gs): "); Serial.println(avgAy, 3);
+  Serial.print("Acceleration Z (Gs): "); Serial.println(avgAz, 3);
+  Serial.print("Gyroscope X (deg/s): "); Serial.println(avgGx, 3);
+  Serial.print("Gyroscope Y (deg/s): "); Serial.println(avgGy, 3);
+  Serial.print("Gyroscope Z (deg/s): "); Serial.println(avgGz, 3);
 }

examples/sensor_fusion/sensor_fusion_compass/sensor_fusion_compass.ino

@@ -0,0 +1,76 @@
+#include <Alfredo_NoU3.h>
+
+float yaw_gyro_deg = 0;
+float yaw_mag_deg = 0;
+float wrapped_yaw_mag_deg = 0;
+float yaw = 0;
+
+const float alpha = 0.98; // Complementary filter weighting
+unsigned long lastTime = 0;
+
+float gyro_z_offset_degrees = 0.444;
+
+void setup() {
+  Serial.begin(115200);
+  NoU3.begin();
+
+}
+
+void loop() {
+
+  if (NoU3.updateIMUs()) {
+
+    unsigned long currentTime = millis();
+    float dt = (currentTime - lastTime) / 1000.0;
+    lastTime = currentTime;
+
+    // Gyroscope yaw update
+    yaw_gyro_deg += (NoU3.gyroscope_z - gyro_z_offset_degrees) * dt;
+
+    // Magnetometer yaw
+    yaw_mag_deg = -1 * degrees(atan2(NoU3.magnetometer_y, NoU3.magnetometer_x));
+
+    // Wrap Magnetometer yaw
+    wrapped_yaw_mag_deg = wrapYaw(yaw_mag_deg);
+
+    // Apply complementary filter with drift compensation
+    yaw = alpha * (yaw_gyro_deg) + (1 - alpha) * wrapped_yaw_mag_deg;
+
+    // Print results
+    Serial.print("yaw_gyro_deg: ");
+    Serial.print(yaw_gyro_deg);
+    Serial.print(" wrapped_yaw_mag_deg: ");
+    Serial.print(wrapped_yaw_mag_deg);
+    Serial.print(" Yaw: ");
+    Serial.print(yaw);
+
+    Serial.println(" ");
+
+  }
+}
+
+// Function to process yaw and keep track of rotations
+float wrapYaw(float currentYaw) {
+  static bool isInitialized = false;
+  static int rotations = 0;
+  static float previousYaw = 0;
+
+  if(isInitialized == false){
+    previousYaw = currentYaw;
+    isInitialized = true;
+  }
+
+    // Check for wrapping
+    if (currentYaw - previousYaw > 180.0) {
+        rotations--; // Wrapped from -180 to 180
+    } else if (currentYaw - previousYaw < -180.0) {
+        rotations++; // Wrapped from 180 to -180
+    }
+
+    // Wrap the yaw angle between -180 and 180
+    float wrappedYaw = currentYaw + rotations * 360.0;
+
+    previousYaw = currentYaw;
+
+    return wrappedYaw;
+}

library.properties

@@ -1,5 +1,5 @@
 name=Alfredo-NoU3
-version=1.0.5
+version=1.0.6
 author=Alfredo Systems
 maintainer=Jacob Williams ([email protected])
 sentence=Library for the Alfredo NoU3 robot control board.