Drive

drive/BNO.cpp

@@ -0,0 +1,71 @@
+#include "Arduino.h"
+#include "BNO.h"
+
+BNO::BNO() {
+}
+
+void BNO::iniciar() {
+  if (!bno.begin(OPERATION_MODE_IMUPLUS)) {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while (1);
+  }
+  bno.setExtCrystalUse(true);
+}
+
+void BNO::readValues() {
+
+  sensors_event_t orientationData;
+  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
+  yaw = (double)orientationData.orientation.x;
+  // convert yaw angle to range -180 to 180
+  yaw = (yaw > 0) ? (360 - yaw) : yaw;
+  yaw = (yaw > 180) ? (yaw - 360) : yaw;
+
+  if (offset > 0) {
+    yaw = (yaw > 0) ? (-180 + yaw) : (180 + yaw);
+  }
+  // determine the direction 
+  right = (yaw >= 0) ? false : true;
+}
+
+
+void BNO::displayCalStatus()
+{
+  /* Get the four calibration values (0..3) */
+  /* Any sensor data reporting 0 should be ignored, */
+  /* 3 means 'fully calibrated" */
+  uint8_t system, gyro, accel, mag;
+  system = gyro = accel = mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+
+  /* The data should be ignored until the system calibration is > 0 */
+   Serial.print("\t");
+
+  /* Display the individual values */
+  Serial.print("Sys:");
+  Serial.print(system, DEC);
+  Serial.print(" G:");
+  Serial.print(gyro, DEC);
+  Serial.print(" A:");
+  Serial.print(accel, DEC);
+  Serial.print(" M:");
+  Serial.println(mag, DEC);
+}
+
+
+double BNO::getYaw() {
+  return yaw;
+}
+
+double BNO::getMag() {
+  return mag;
+}
+
+void BNO::setOffset(double off) {
+  offset = off;
+}
+
+bool BNO::isRight() {
+  return right;
+}

drive/BNO.h

@@ -0,0 +1,28 @@
+#ifndef BNO_h
+  #define BNO_h
+
+  #include <Wire.h>
+  #include <Adafruit_Sensor.h>
+  #include <Adafruit_BNO055.h>
+
+  class BNO {
+    private:
+      double yaw;
+      bool right = true;
+      Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);
+      double offset = 0;
+      double mag = 0;
+
+    public:
+
+      BNO();
+      void iniciar();
+      void readValues();
+      double getYaw();
+      double getMag();
+      void setOffset(double off);
+      bool isRight();
+      void displayCalStatus();
+  };
+
+#endif

drive/driveBNO.ino

@@ -0,0 +1,47 @@
+#include "Arduino.h"
+#include "BNO.h" 
+#include "drive.h"
+
+const int m1Speed = 4; 
+const int m1P1 = 23;
+const int m1P2 = 22;
+const int m2Speed = 5;
+const int m2P1 = 24;
+const int m2P2 = 25;
+const int m3Speed = 6;
+const int m3P1 = 26;
+const int m3P2 = 27;
+const int testSpeed = 100;
+drive robotDrive(m1Speed, m1P1, m1P2, m2Speed, m2P1, m2P2, m3Speed, m3P1, m3P2); 
+
+BNO orientationSensor; // Create an instance of the BNO class
+
+void setup() {
+  Serial.begin(9600);
+  orientationSensor.iniciar(); // Initialize the orientation sensor 
+  robotDrive.initialize();  
+}
+
+void loop() {
+  orientationSensor.readValues(); // Read orientation values
+
+  double yaw = orientationSensor.getYaw(); // Get yaw angle
+  double mag = orientationSensor.getMag(); // Get magnetometer data
+  bool right = orientationSensor.isRight(); // Check if yaw angle is to the right
+
+  // Display orientation data
+  Serial.print("Yaw: ");
+  Serial.println(yaw);
+  Serial.print("Magnetometer: ");
+  Serial.println(mag);
+  Serial.print("Right: ");
+  Serial.println(right ? "Yes" : "No");
+
+  // Display calibration status
+  Serial.println("Calibration Status:");
+  orientationSensor.displayCalStatus();  
+
+  robotDrive.giro(testSpeed, yaw);
+
+  delay(1000); // Delay for stability
+}