From 80a01ee00a330636711aa0c0ad78d6ca34d37ec8 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Wed, 27 Nov 2019 17:17:18 +0100
Subject: [PATCH] Finally us working

---
 include/data_source.h      |  2 +-
 include/data_source_ball.h |  2 +-
 include/data_source_us.h   |  6 ++---
 src/data_source.cpp        |  4 ++--
 src/data_source_us.cpp     | 49 ++++++++++++++++++++++++++++++++++----
 src/main.cpp               |  8 ++-----
 src/sensors.cpp            |  9 ++++---
 7 files changed, 57 insertions(+), 23 deletions(-)

diff --git a/include/data_source.h b/include/data_source.h
index d437c2e..6d51e0c 100644
--- a/include/data_source.h
+++ b/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: 
diff --git a/include/data_source_ball.h b/include/data_source_ball.h
index 49b7970..dc6da55 100644
--- a/include/data_source_ball.h
+++ b/include/data_source_ball.h
@@ -10,4 +10,4 @@ class DataSourceBall : public DataSource{
         
         int angle,distance;
         bool ballSeen;
-};
\ No newline at end of file
+}; 
\ No newline at end of file
diff --git a/include/data_source_us.h b/include/data_source_us.h
index 3eabc9f..5e9e49a 100644
--- a/include/data_source_us.h
+++ b/include/data_source_us.h
@@ -7,15 +7,15 @@ 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;                
         long us_t1;                
-        bool us_flag;             
+        bool us_flag;       
 };
\ No newline at end of file
diff --git a/src/data_source.cpp b/src/data_source.cpp
index 3e9d73f..e629df8 100644
--- a/src/data_source.cpp
+++ b/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){
diff --git a/src/data_source_us.cpp b/src/data_source_us.cpp
index d8adf6a..a214de9 100644
--- a/src/data_source_us.cpp
+++ b/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);
   }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 5c97aba..ec75c86 100644
--- a/src/main.cpp
+++ b/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());*/
 }
\ No newline at end of file
diff --git a/src/sensors.cpp b/src/sensors.cpp
index f5f6e64..23bdeec 100644
--- a/src/sensors.cpp
+++ b/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();