From 7dd994e00c6eff63c98d7500f5647b16b8f3ca1f Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Mon, 18 Nov 2019 14:37:55 +0100
Subject: [PATCH] adding a bit of polymorphism

---
 include/data_source.h        | 18 +++++++++++++-----
 include/data_source_bno055.h |  2 +-
 include/drivecontroller.h    |  5 +++--
 include/sensors.h            |  2 +-
 src/data_source.cpp          | 14 ++++++--------
 src/data_source_bno055.cpp   | 16 ++++++++--------
 src/drivecontroller.cpp      |  8 ++++++--
 src/main.cpp                 |  5 +++--
 8 files changed, 41 insertions(+), 29 deletions(-)

diff --git a/include/data_source.h b/include/data_source.h
index 33ec4a6..c995c08 100644
--- a/include/data_source.h
+++ b/include/data_source.h
@@ -14,14 +14,21 @@ class DataSource {
         DataSource(int, bool);
 
     public: 
-        void update();
-        void test();
-        void readSensor();
-        void postProcess();
-        int getValue();
+        virtual void update();
+        virtual void test();
+        virtual void readSensor();
+        virtual void postProcess();
+        virtual int getValue();
 
 
     public:
+        enum Protocols {
+            P_I2C,
+            P_RXTX,
+            P_APIN,
+            P_PIND            
+        };
+        
         HardwareSerial* ser;
         TwoWire* i2c;
 
@@ -30,4 +37,5 @@ class DataSource {
         int value;
         
 
+
 };
\ No newline at end of file
diff --git a/include/data_source_bno055.h b/include/data_source_bno055.h
index b93fba7..36a01c0 100644
--- a/include/data_source_bno055.h
+++ b/include/data_source_bno055.h
@@ -8,7 +8,7 @@ class DataSourceBNO055 : public DataSource{
 
     public:
         DataSourceBNO055();
-        void readSensor();
+        void readSensor() override;
         
     public:
         Adafruit_BNO055 bno;
diff --git a/include/drivecontroller.h b/include/drivecontroller.h
index 0667471..93dc6eb 100644
--- a/include/drivecontroller.h
+++ b/include/drivecontroller.h
@@ -4,9 +4,9 @@
 #include "motor.h"
 
 //PID Constants
-#define KP 0.7
+#define KP 0.9
 #define KI 0 
-#define KD 0.7
+#define KD 1.1
 #define DEADZONE_MIN 25
 
 class DriveController{
@@ -19,6 +19,7 @@ class DriveController{
         void prepareDrive(int dir, int speed, int tilt);
         void drivePrepared();
         float updatePid();
+        float torad(float f);
 
     private:
         Motor* m1;
diff --git a/include/sensors.h b/include/sensors.h
index a31ba70..7473793 100644
--- a/include/sensors.h
+++ b/include/sensors.h
@@ -12,5 +12,5 @@
 void initSensors();
 void updateSensors();
 
-extr DataSourceBNO055* compass;
+extr DataSource* compass;
 extr DriveController* drive;
\ No newline at end of file
diff --git a/src/data_source.cpp b/src/data_source.cpp
index 00c958a..e72c70d 100644
--- a/src/data_source.cpp
+++ b/src/data_source.cpp
@@ -6,7 +6,7 @@ DataSource::DataSource(void){
 
 DataSource::DataSource(TwoWire i2c_){
     
-    i2c = &(i2c_);
+    this->i2c = &(i2c_);
     protocol = 1;
     
     i2c->begin();
@@ -14,14 +14,14 @@ DataSource::DataSource(TwoWire i2c_){
 }
 
 DataSource::DataSource(HardwareSerial ser_, int baud){
-    ser = &(ser_);
+    this->ser = &(ser_);
     protocol = 2;
     
     ser->begin(baud);
 }
 
 DataSource::DataSource(int pin_, bool analog){
-    pin = pin_;
+    this->pin = pin_;
     if(analog) protocol = 3;
     else protocol = 4;
 }
@@ -35,9 +35,7 @@ void DataSource::update(){
     postProcess();
 }
 
-void DataSource::postProcess(){
-
-}
+void DataSource::postProcess(){ }
 
 void DataSource::readSensor(){
     if(protocol == 1) value = i2c->read();
@@ -47,6 +45,6 @@ void DataSource::readSensor(){
 }
 
 void DataSource::test(){
-    update();
-    DEBUG_PRINT.println(getValue());
+    this->update();
+    DEBUG_PRINT.println(this->getValue());
 }
\ No newline at end of file
diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp
index 4725bd2..9d37b2a 100644
--- a/src/data_source_bno055.cpp
+++ b/src/data_source_bno055.cpp
@@ -3,16 +3,16 @@
 //bool loaded = false;
 
 DataSourceBNO055::DataSourceBNO055(){
-    bno = Adafruit_BNO055();
-    bno.begin(bno.OPERATION_MODE_IMUPLUS);  //Posizione impostata a P7 alle righe 105,107 di Adafruit_BNO55.cpp
-    bno.setExtCrystalUse(true);
-    //loaded = true;
-    value = 0;
+  // protocol = Protocols.P_I2C;
+  
+  bno = Adafruit_BNO055();
+  bno.begin(bno.OPERATION_MODE_IMUPLUS);  //Posizione impostata a P7 alle righe 105,107 di Adafruit_BNO55.cpp
+  bno.setExtCrystalUse(true);
+  //loaded = true;
+  value = 0;
 }
 
 void DataSourceBNO055::readSensor(){
-// if(loaded){
   imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
-  value = (int) euler.x();
-//  }
+  this->value = (int) euler.x();
 }
diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp
index 5e801df..85fa612 100644
--- a/src/drivecontroller.cpp
+++ b/src/drivecontroller.cpp
@@ -8,8 +8,8 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     m4 = m4_;
     
     for(int i = 0; i < 360; i++){
-        sins[i] = (float) sin(i);
-        cosins[i] =  (float) cos(i);
+        sins[i] = (float) sin(torad(i));
+        cosins[i] =  (float) cos(torad(i));
     }
 }
 
@@ -23,6 +23,10 @@ void DriveController::drivePrepared(){
     drive(pDir, pSpeed, pTilt);
 }
 
+float DriveController::torad(float f){
+    return (f * PI / 180.0);
+}
+
 void DriveController::drive(int dir, int speed, int tilt){
     vx = ((speed * cosins[dir]));
     vy = ((-speed * sins[dir]));
diff --git a/src/main.cpp b/src/main.cpp
index a5afbac..aacf1b0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,14 +6,15 @@
 
 
 void setup() {
+  delay(1000);
+
   DEBUG_PRINT.begin(9600);
+  
   initSensors();
 }
 
 void loop() {
   updateSensors();
-
   //should recenter using predefined values
   drive->drive(0, 0, 0);
-  DEBUG_PRINT.println(compass->getValue());
 }
\ No newline at end of file