finished us, having trouble testing

include/data_source_ball.h

@@ -7,6 +7,7 @@ class DataSourceBall : public DataSource{
         DataSourceBall(HardwareSerial* ser, int baud);
         void postProcess() override;
         void test() override;
+        
         int angle,distance;
         bool ballSeen;
 };

include/data_source_us.h

@@ -0,0 +1,21 @@
+#pragma once
+#include "data_source.h"
+
+class DataSourceUS : public DataSource{
+
+    public:
+        DataSourceUS(TwoWire* i2c_);
+        void postProcess() override;
+        void test() override;
+        void usMode();
+        void usTrigger();
+        void usReceive();
+        //void readSensor() override;
+
+        int reading;
+        long us_t0;                
+        long us_t1;                
+        bool us_flag;              
+        int us_values[4];             
+        int us_sx, us_dx, us_px, us_fr;      
+};

include/sensors.h

@@ -2,6 +2,7 @@
 #include "data_source_bno055.h"
 #include "data_source_ball.h"
 #include "data_source_camera.h"
+#include "data_source_us.h"
 #include "motor.h"
 #include "drivecontroller.h"
 
@@ -17,4 +18,5 @@ void updateSensors();
 extr DataSource* compass;
 extr DataSource* ball;
 extr DataSource* camera;
+extr DataSource* us;
 extr DriveController* drive;

src/data_source_us.cpp

@@ -0,0 +1,84 @@
+#include "data_source_us.h"
+#include "vars.h"
+
+DataSourceUS :: DataSourceUS(TwoWire* i2c_) : DataSource(i2c_) { 
+    i2c->begin();   
+}
+
+
+void DataSourceUS :: postProcess(){
+    usMode();
+    us_fr = us_values[0]; // FRONT US
+    us_dx = us_values[1]; // DX US
+    us_px = us_values[2]; // BACK US
+    us_sx = us_values[3]; // SX US
+}
+
+void DataSourceUS :: usMode(){
+    if (us_flag == false) {
+        usTrigger();
+        us_flag = true;
+        us_t0 = millis();
+    } else {
+        us_t1 = millis();
+        if ((us_t1 - us_t0) > 70) {
+        usReceive();
+        us_flag = false;
+    }
+  }
+}
+
+void DataSourceUS :: usTrigger(){
+    for (int i = 0; i < 4; i++){
+        i2c->beginTransmission(112 + i);
+        i2c->write(byte(0x00));
+        i2c->write(byte(0x51));
+        i2c->endTransmission();
+    }
+}
+
+void DataSourceUS :: usReceive(){
+    for (int i = 0; i < 4; i++){
+        i2c->beginTransmission(112 + i);
+        i2c->write(byte(0x00));
+        i2c->write(byte(0x51));
+        i2c->endTransmission();
+        delay(70);
+        i2c->requestFrom(112 + i, 2);
+        if(2 <= i2c->available()){
+            reading = i2c->read();
+            reading = reading << 8;
+            reading |= i2c->read();
+            us_values[i] = reading;
+        }
+    }
+}
+
+void DataSourceUS :: test(){
+    for (int i = 0; i < 4; i++){
+        i2c->beginTransmission(112 + i);
+        i2c->write(byte(0x00));
+        i2c->write(byte(0x51));
+        i2c->endTransmission();
+        delay(70);
+        i2c->requestFrom(112 + i, 2);
+        if(2 <= i2c->available()){
+            reading = i2c->read();
+            reading = reading << 8;
+            reading |= i2c->read();
+            us_values[i] = reading;
+        }
+    }
+    us_fr = us_values[0];
+    us_dx = us_values[1];
+    us_px = us_values[2];
+    us_sx = us_values[3];
+
+    //printing the result
+    DEBUG_PRINT.println("---------------------");
+    for (int i = 0; i < 4; i++) {
+    DEBUG_PRINT.println(us_values[i]);
+    delay(250);
+    }
+    DEBUG_PRINT.println("---------------------");
+}

src/main.cpp

@@ -18,6 +18,7 @@ void loop() {
   //should recenter using predefined values
  // drive->drive(0, 0, 0);
  //compass->test();
-  camera->test();
+ //camera->test();
+ us->test();
   delay(100);
 }

src/sensors.cpp

@@ -6,6 +6,7 @@ void initSensors(){
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial4, 57600);
     camera = new DataSourceCamera(&Serial2, 19200);
+    us = new DataSourceUS(&Wire1);
 }
 
 void updateSensors(){
@@ -16,4 +17,5 @@ void updateSensors(){
     compass->update();
     ball->update();
     camera->update();
+    us->update();
 }