now latest complete structs are copied into new ones when updating status vector

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
 

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

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

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