From cee576e9c8a9703aac9d07e6364f5e7c80c2db62 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Mon, 17 Feb 2020 14:34:39 +0100
Subject: [PATCH] now latest complete structs are copied into new ones when
 updating status vector

---
 include/drivecontroller.h  | 2 +-
 src/data_source_bno055.cpp | 4 ++--
 src/main.cpp               | 2 --
 src/status_vector.cpp      | 2 ++
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/include/drivecontroller.h b/include/drivecontroller.h
index 5fcdc70..6b7b5e2 100755
--- a/include/drivecontroller.h
+++ b/include/drivecontroller.h
@@ -7,7 +7,7 @@
 //PID Constants
 #define KP 1.2
 #define KI 0.0
-#define KD 0.5
+#define KD 0.0
 
 #define UNLOCK_THRESH 800
 
diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp
index e6782be..e94408b 100755
--- a/src/data_source_bno055.cpp
+++ b/src/data_source_bno055.cpp
@@ -18,7 +18,7 @@ void DataSourceBNO055::readSensor(){
     imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     this->value = (int) euler.x();
     lastTime = millis();
+    CURRENT_INPUT_WRITE.IMUAngle = this->value;
+    CURRENT_DATA_WRITE.IMUAngle = this->value;
   }
-  CURRENT_INPUT_WRITE.IMUAngle = this->value;
-  CURRENT_DATA_WRITE.IMUAngle = this->value;
 }
diff --git a/src/main.cpp b/src/main.cpp
index f947985..0459db1 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,8 +22,6 @@ void loop() {
   goalie->play(role==1);
   keeper->play(role==0);  
 
-  Serial.println(CURRENT_DATA_READ.IMUAngle);
-
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  
   updateStatusVector();
diff --git a/src/status_vector.cpp b/src/status_vector.cpp
index dee265e..4475bdf 100644
--- a/src/status_vector.cpp
+++ b/src/status_vector.cpp
@@ -12,4 +12,6 @@ void initStatusVector(){
 
 void updateStatusVector(){
     currentSVIndex = (currentSVIndex+1) % dim;
+    CURRENT_DATA_WRITE = CURRENT_DATA_READ;
+    CURRENT_INPUT_WRITE = CURRENT_INPUT_READ;
 }
\ No newline at end of file