first commit

src/main.cpp

@@ -0,0 +1,116 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include "Adafruit_BNO08x_RVC.h"
+#include <PixyI2C.h>
+#include "Ultrasonic.h"
+
+PixyI2C pixy;
+Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
+Ultrasonic uSensor1 = Ultrasonic(7);
+Ultrasonic uSensor2 = Ultrasonic(10);
+Ultrasonic uSensor3 = Ultrasonic(9);
+
+void setup()
+{
+  Wire.begin();
+  pixy.init();
+
+  // Wait for serial monitor to open
+  Serial.begin(115200);
+  while (!Serial)
+    delay(10);
+
+  Serial1.begin(115200); // This is the baud rate specified by the datasheet
+  while (!Serial1)
+    delay(10);
+
+  if (!rvc.begin(&Serial1))
+  { // connect to the sensor over hardware serial
+    while (1)
+      delay(10);
+  }
+  // Print labels for the values
+  Serial.print(F("Yaw"));
+  Serial.print(F("\tPitch"));
+  Serial.print(F("\tRoll"));
+  Serial.print(F("\tX"));
+  Serial.print(F("\tY"));
+  Serial.println(F("\tZ"));
+}
+
+void loop()
+{
+  // Get IMU data
+  Wire.requestFrom(0x10 / 2, 2);
+  BNO08x_RVC_Data heading;
+  static int i = 0;
+  uint16_t blocks;
+  char buf[32];
+
+  while (rvc.read(&heading))
+  {
+    int c = Wire.read(); // direction is the first byte
+    int strength = Wire.read();
+    // Print IMU data
+    Serial.print("IMU - Direction: ");
+    Serial.print(c);
+    Serial.print(" Strength: ");
+    Serial.println(strength);
+    Serial.print("Heading: ");
+    Serial.print(heading.yaw);
+    Serial.print(",");
+    Serial.print(heading.pitch);
+    Serial.print(",");
+    Serial.print(heading.roll);
+    Serial.println();
+    Serial.print("Acceleration: ");
+    Serial.print(heading.x_accel);
+    Serial.print(",");
+    Serial.print(heading.y_accel);
+    Serial.print(",");
+    Serial.print(heading.z_accel);
+    Serial.println();
+
+    // Get Ultrasonic sensor data
+    long uS1 = uSensor1.MeasureInCentimeters();
+    Serial.print("Distance uS1: ");
+    Serial.print(uS1);
+    Serial.println(" cm");
+
+    long uS2 = uSensor2.MeasureInCentimeters();
+    Serial.print("Distance uS2: ");
+    Serial.print(uS2);
+    Serial.println(" cm");
+
+    long uS3 = uSensor3.MeasureInCentimeters();
+    Serial.print("Distance uS3: ");
+    Serial.print(uS3);
+    Serial.println(" cm");
+
+    // Get Pixy blocks
+    blocks = pixy.getBlocks();
+
+    if (blocks)
+    {
+      Serial.println("Detected blocks");
+      i++;
+      // Print every 50 frames to avoid overwhelming serial
+      if (i % 50 == 0)
+      {
+
+        // Print Pixy blocks
+        sprintf(buf, "Detected %d blocks:\n", blocks);
+        Serial.print(buf);
+        for (int j = 0; j < blocks; j++)
+        {
+          sprintf(buf, "  block %d: ", j);
+          Serial.print(buf);
+          pixy.blocks[j].print();
+        }
+        Serial.println();
+      }
+
+      delay(250);
+    }
+  }
+}