From 076a14e3e60cdc833bb9bdf65895d44c9d08a7b2 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Mon, 25 Nov 2019 17:30:47 +0100
Subject: [PATCH] finished us, having trouble testing

---
 include/data_source_ball.h |  1 +
 include/data_source_us.h   | 21 ++++++++++
 include/sensors.h          |  2 +
 src/data_source_ball.cpp   |  2 +-
 src/data_source_us.cpp     | 84 ++++++++++++++++++++++++++++++++++++++
 src/main.cpp               |  3 +-
 src/sensors.cpp            |  2 +
 7 files changed, 113 insertions(+), 2 deletions(-)

diff --git a/include/data_source_ball.h b/include/data_source_ball.h
index 4199888..49b7970 100644
--- a/include/data_source_ball.h
+++ b/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;
 };
\ No newline at end of file
diff --git a/include/data_source_us.h b/include/data_source_us.h
index e69de29..6ba8788 100644
--- a/include/data_source_us.h
+++ b/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;      
+};
\ No newline at end of file
diff --git a/include/sensors.h b/include/sensors.h
index 9a3a690..806e68c 100644
--- a/include/sensors.h
+++ b/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;
diff --git a/src/data_source_ball.cpp b/src/data_source_ball.cpp
index bf4cd39..83dda85 100644
--- a/src/data_source_ball.cpp
+++ b/src/data_source_ball.cpp
@@ -1,7 +1,7 @@
 #include "data_source_ball.h"
 #include "vars.h"
 
-DataSourceBall::DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) {
+DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) {
 }
 
 void DataSourceBall :: postProcess(){
diff --git a/src/data_source_us.cpp b/src/data_source_us.cpp
index e69de29..5bc1d8a 100644
--- a/src/data_source_us.cpp
+++ b/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("---------------------");
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 96dbd77..5c97aba 100644
--- a/src/main.cpp
+++ b/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);
 }
\ No newline at end of file
diff --git a/src/sensors.cpp b/src/sensors.cpp
index 7305d70..08f878f 100644
--- a/src/sensors.cpp
+++ b/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();
 }
\ No newline at end of file