From 66b82e5ee06b41517fed4ad1a705b0d0661a9509 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@gmail.com>
Date: Mon, 21 Oct 2019 15:30:28 +0200
Subject: [PATCH] New code working with imu reading, tests are not working
 currently

---
 include/data_source_bno055.h |  1 -
 src/data_source.cpp          |  6 +++++-
 src/data_source_bno055.cpp   | 13 ++++++++-----
 src/main.cpp                 | 10 +++++++++-
 4 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/include/data_source_bno055.h b/include/data_source_bno055.h
index dbb4ede..ad7e9c0 100644
--- a/include/data_source_bno055.h
+++ b/include/data_source_bno055.h
@@ -7,7 +7,6 @@ class DataSourceBNO055 : public DataSource{
     public:
         DataSourceBNO055();
         void readSensor();
-        void postProcess();
         
     public:
         Adafruit_BNO055 bno;
diff --git a/src/data_source.cpp b/src/data_source.cpp
index 28f4418..00c958a 100644
--- a/src/data_source.cpp
+++ b/src/data_source.cpp
@@ -35,6 +35,10 @@ void DataSource::update(){
     postProcess();
 }
 
+void DataSource::postProcess(){
+
+}
+
 void DataSource::readSensor(){
     if(protocol == 1) value = i2c->read();
     else if(protocol == 2) while(ser->available() > 0) value = ser->read();
@@ -44,5 +48,5 @@ void DataSource::readSensor(){
 
 void DataSource::test(){
     update();
-    DEBUG_PRINT.println(value);
+    DEBUG_PRINT.println(getValue());
 }
\ No newline at end of file
diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp
index c9192ba..d4e6e2d 100644
--- a/src/data_source_bno055.cpp
+++ b/src/data_source_bno055.cpp
@@ -1,15 +1,18 @@
 #include "data_source_bno055.h"
 
+//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;
 }
 
 void DataSourceBNO055::readSensor(){
- imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
-
-    if (euler.x() != value) {
-      value = euler.x();
-    }
+// if(loaded){
+  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
+  value = (int) euler.x();
+//  }
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index fae2d5e..a1927ce 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,12 +1,20 @@
 #include <Arduino.h>
 #include "vars.h"
+#include "data_source_bno055.h"
 // #include <imu_class/imu.cpp>
 //cyao c:
 
+DataSourceBNO055* compass;
+
 void setup() {
   DEBUG_PRINT.begin(9600);
+  compass = new DataSourceBNO055();
 }
 
 void loop() {
-  
+  compass->readSensor();
+  DEBUG_PRINT.println(compass->getValue());
+
+  //DEBUG_PRINT.println("Ciaooo");
+  delay(100);
 }
\ No newline at end of file