adding a bit of polymorphism

include/data_source.h

@@ -14,14 +14,21 @@ class DataSource {
         DataSource(int, bool);
 
     public: 
-        void update();
+        virtual void update();
-        void test();
+        virtual void test();
-        void readSensor();
+        virtual void readSensor();
-        void postProcess();
+        virtual void postProcess();
-        int getValue();
+        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;
         
 
+
 };

include/data_source_bno055.h

@@ -8,7 +8,7 @@ class DataSourceBNO055 : public DataSource{
 
     public:
         DataSourceBNO055();
-        void readSensor();
+        void readSensor() override;
         
     public:
         Adafruit_BNO055 bno;

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;

include/sensors.h

@@ -12,5 +12,5 @@
 void initSensors();
 void updateSensors();
 
-extr DataSourceBNO055* compass;
+extr DataSource* compass;
 extr DriveController* drive;

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();
+    this->update();
-    DEBUG_PRINT.println(getValue());
+    DEBUG_PRINT.println(this->getValue());
 }

src/data_source_bno055.cpp

@@ -3,6 +3,8 @@
 //bool loaded = false;
 
 DataSourceBNO055::DataSourceBNO055(){
+  // 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);
@@ -11,8 +13,6 @@ DataSourceBNO055::DataSourceBNO055(){
 }
 
 void DataSourceBNO055::readSensor(){
-// if(loaded){
   imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
-  value = (int) euler.x();
+  this->value = (int) euler.x();
-//  }
 }

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);
+        sins[i] = (float) sin(torad(i));
-        cosins[i] =  (float) cos(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]));

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());
 }