Finally us working

include/data_source.h

@@ -10,7 +10,7 @@ class DataSource {
     public:
         DataSource();
         DataSource(HardwareSerial*, int);
-        DataSource(TwoWire*, int addr);
+        DataSource(TwoWire* , int);
         DataSource(int, bool);
 
     public: 

include/data_source_us.h

@@ -7,12 +7,12 @@ class DataSourceUS : public DataSource{
 
     public:
         DataSourceUS(TwoWire* i2c_, int addr);
-        void postProcess() override;
         void test() override;
+        void readSensor() override;
+        
         void usMode();
         void usTrigger();
         void usReceive();
-        void readSensor() override;
 
         int reading;
         long us_t0;                

src/data_source.cpp

@@ -9,8 +9,8 @@ DataSource::DataSource(TwoWire* i2c_, int addr){
     this->i2c = i2c_;    
     this->i2CAddr = addr;
 
-    i2c->end();
-    i2c->begin();
+    this->i2c->end();
+    this->i2c->begin();
 }
 
 DataSource::DataSource(HardwareSerial* ser_, int baud){

src/data_source_us.cpp

@@ -1,13 +1,51 @@
 #include "data_source_us.h"
 #include "vars.h"
 
-DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr) { }
+DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr){
+  us_flag = false;
+}
 
 void DataSourceUS::test(){
-    readSensor();
-    delay(US_WAIT_TIME + 5);
-    readSensor();
-    DEBUG_PRINT.println(value);
+  
+  // test from srf02_example
+    // step 1: instruct sensor to read echoes
+    // transmit to device #112 (0x70)
+    Wire1.beginTransmission(i2CAddr);
+    // sets register pointer to the command register (0x00)
+    Wire1.write(byte(0x00));
+    // command sensor to measure in "centimeters" (0x51). 0x50 inches and 0x52
+    // microseconds
+    Wire1.write(byte(0x51));
+    // stop transmitting
+    Wire1.endTransmission();
+    // step 2: wait for readings to happen
+    // datasheet suggests at least 65 milliseconds
+    delay(70);
+
+    // step 3: instruct sensor to return a particular echo reading
+    // transmit to device #112
+    Wire1.beginTransmission(i2CAddr);
+    // sets register pointer to echo #1 register (0x02)
+    Wire1.write(byte(0x02));
+    Wire1.endTransmission();
+
+    // step 4: request reading from sensor
+    // request 2 bytes from slave device #112
+    Wire1.requestFrom(i2CAddr, 2);
+
+    // step 5: receive reading from sensor
+    if (2 <= Wire1.available()) {
+      // if two bytes were received
+      // receive high byte (overwrites  previous reading)
+      reading = Wire1.read();
+      // shift high byte to be high 8 bits
+      reading = reading << 8;
+      // receive low byte as lower 8 bit
+      reading |= Wire1.read();
+      value = reading;
+    }
+
+  DEBUG_PRINT.println(value);
 }
 
 void DataSourceUS::readSensor(){
@@ -55,4 +93,5 @@ void DataSourceUS::usReceive() {
     // receive low byte as lower 8 bit
     reading |= i2c->read();
     value = reading;
+    DEBUG_PRINT.println(reading);
   }

src/main.cpp

@@ -15,10 +15,6 @@ void setup() {
 
 void loop() {
   updateSensors();
-  //should recenter using predefined values
- // drive->drive(0, 0, 0);
- //compass->test();
- //camera->test();
- us->test();
-  delay(100);
+  /*if(millis() % 100 == 0)
+  DEBUG_PRINT.println(us->getValue());*/
 }

src/sensors.cpp

@@ -2,18 +2,17 @@
 #include "sensors.h"
 
 void initSensors(){
+    pinMode(SWITCH_DX, INPUT);
+    pinMode(SWITCH_SX, INPUT);
+
     drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial4, 57600);
     camera = new DataSourceCamera(&Serial2, 19200);
-    us = new DataSourceUS(&Wire1, 112);
+    us = new DataSourceUS(&Wire1, int(113));
 }
 
 void updateSensors(){
-    
-    pinMode(SWITCH_DX, INPUT);
-    pinMode(SWITCH_SX, INPUT);
-
     compass->update();
     ball->update();
     camera->update();