From 9c3ea8d602de4e7db96e1a115b4b49de56599ec7 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Mon, 2 Dec 2019 14:50:59 +0100
Subject: [PATCH] US sensors using DataSourceController now working

---
 .vscode/settings.json            |  3 ++-
 include/data_source_controller.h | 22 +++++++++++++++++++
 include/sensors.h                |  4 +++-
 src/data_source_controller.cpp   | 37 ++++++++++++++++++++++++++++++++
 src/main.cpp                     |  2 ++
 src/sensors.cpp                  |  6 ++++--
 6 files changed, 70 insertions(+), 4 deletions(-)
 create mode 100644 include/data_source_controller.h
 create mode 100644 src/data_source_controller.cpp

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 77b68ea..ec0f11d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -19,6 +19,7 @@
         "unordered_map": "cpp",
         "vector": "cpp",
         "string_view": "cpp",
-        "initializer_list": "cpp"
+        "initializer_list": "cpp",
+        "*.tcc": "cpp"
     }
 }
\ No newline at end of file
diff --git a/include/data_source_controller.h b/include/data_source_controller.h
new file mode 100644
index 0000000..90b6c3c
--- /dev/null
+++ b/include/data_source_controller.h
@@ -0,0 +1,22 @@
+#include "data_source.h"
+#include "vars.h"
+#include <Arduino.h>
+#include <vector>
+
+using namespace std;
+class DataSourceController {
+
+    public:
+        DataSourceController();
+        DataSourceController(vector<DataSource*>);
+
+    public:
+        void update();
+        void test();
+        void postProcess();
+        void readSensor();
+        void getValue();
+
+        vector<DataSource*> ds;
+
+};
\ No newline at end of file
diff --git a/include/sensors.h b/include/sensors.h
index 806e68c..b5e1ad1 100644
--- a/include/sensors.h
+++ b/include/sensors.h
@@ -4,6 +4,7 @@
 #include "data_source_camera.h"
 #include "data_source_us.h"
 #include "motor.h"
+#include "data_source_controller.h"
 #include "drivecontroller.h"
 
 #ifdef SENSORS_CPP
@@ -18,5 +19,6 @@ void updateSensors();
 extr DataSource* compass;
 extr DataSource* ball;
 extr DataSource* camera;
-extr DataSource* us;
+//extr DataSource* us;
+extr DataSourceController* usCtrl;
 extr DriveController* drive;
diff --git a/src/data_source_controller.cpp b/src/data_source_controller.cpp
new file mode 100644
index 0000000..88db1f8
--- /dev/null
+++ b/src/data_source_controller.cpp
@@ -0,0 +1,37 @@
+#include "data_source_controller.h"
+
+using namespace std;
+
+DataSourceController::DataSourceController() {}
+DataSourceController::DataSourceController(vector<DataSource*> ds_){
+    this->ds = ds_;
+}
+
+void DataSourceController::readSensor(){
+    for(DataSource* d : ds){
+        d->readSensor();
+    }
+}
+void DataSourceController::update(){
+    for(DataSource* d : ds){
+        d->update();
+    }
+}
+void DataSourceController::postProcess(){
+    for(DataSource* d : ds){
+        d->postProcess();
+    }
+}
+void DataSourceController::getValue(){
+    for(DataSource* d : ds){
+        d->getValue();
+    }
+}
+void DataSourceController::test(){
+    DEBUG_PRINT.println("========================================");
+    for(DataSource* d : ds){
+        d->test();
+    }
+    DEBUG_PRINT.println("========================================");
+
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index ec75c86..cc0a32e 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -17,4 +17,6 @@ void loop() {
   updateSensors();
   /*if(millis() % 100 == 0)
   DEBUG_PRINT.println(us->getValue());*/
+  usCtrl->test();
+  delay(200);
 }
\ No newline at end of file
diff --git a/src/sensors.cpp b/src/sensors.cpp
index 23bdeec..b14aeec 100644
--- a/src/sensors.cpp
+++ b/src/sensors.cpp
@@ -4,17 +4,19 @@
 void initSensors(){
     pinMode(SWITCH_DX, INPUT);
     pinMode(SWITCH_SX, INPUT);
+    vector<DataSource*> dUs { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), 
+    new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) };
 
     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, int(113));
+    usCtrl = new DataSourceController(dUs);
 }
 
 void updateSensors(){
     compass->update();
     ball->update();
     camera->update();
-    us->update();
+    // usCtrl->test();
 }
\ No newline at end of file