status vector not quite working

include/data_source.h

@@ -4,6 +4,7 @@
 #include "Arduino.h"
 #include "HardwareSerial.h"
 #include "vars.h"
+#include "status_vector.h"
 
 class DataSource {
 

include/status_vector.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <Arduino.h>
+
+#ifdef STATUS_VECTOR_CPP
+#define sv_extr 
+#else
+#define sv_extr extern
+#endif
+
+#define dim 20
+#define CURRENT_DATA_READ (datas[(currentSVIndex-1) % dim])
+#define CURRENT_DATA_WRITE (datas[(currentSVIndex) % dim])
+#define CURRENT_INPUT_READ (inputs[(currentSVIndex-1) % dim])
+#define CURRENT_INPUT_WRITE (inputs[(currentSVIndex) % dim])
+
+typedef struct input{
+    int IMUAngle, USfr, USsx, USdx, USrr, BT;
+    byte ballByte, cameraByte, lineByte;
+    bool SW_DX, SW_SX;
+}input;
+
+typedef struct data{
+    int IMUAngle, ballAngle, ballDistance, cameraAngle, cameraDistance,
+        speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role;
+    byte xb, yb, xy, yy, lineSeen, lineActive;
+    bool mate, ATKgoal, DEFgoal;
+}data;
+
+sv_extr input inputs[dim];
+sv_extr data datas[dim];
+sv_extr int currentSVIndex;
+
+void initStatusVector();
+void updateStatusVector();

src/data_source_bno055.cpp

@@ -17,6 +17,10 @@ void DataSourceBNO055::readSensor(){
   if(millis() - lastTime > DATA_CLOCK){
     imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     this->value = (int) euler.x();
+
+    CURRENT_INPUT_WRITE.IMUAngle = this->value;
+    CURRENT_DATA_WRITE.IMUAngle = this->value;
+
     lastTime = millis();
   }
 }

src/goalie.cpp

@@ -45,8 +45,8 @@ void Goalie::goalie(int plusang) {
 }
 
 void Goalie::storcimentoPorta() {
-  if (camera->getValueAtk(false) >= 10 && camera->getValueAtk(false) <= 90) cstorc+=9;
-  else if (camera->getValueAtk(false)  <= 350 && camera->getValueAtk(false) >= 270) cstorc-=9;
+  if (camera->getValueAtk(true ) >= 10 && camera->getValueAtk(true) <= 90) cstorc+=9;
+  else if (camera->getValueAtk(true)  <= 350 && camera->getValueAtk(true) >= 270) cstorc-=9;
   // else cstorc *= 0.7;
   cstorc = constrain(cstorc, -45, 45);
 }

src/main.cpp

@@ -2,11 +2,13 @@
 
 #include "sensors.h"
 #include "games.h"
+#include "status_vector.h"
 
 void setup() {
   delay(1500);
 
   DEBUG.begin(9600);
+  initStatusVector();
   initSensors();
   initGames();
   
@@ -16,11 +18,11 @@ void setup() {
 
 void loop() {
   updateSensors();
-  camera->test();
-  // compass->test();
+
   goalie->play(role==1);
   keeper->play(role==0);  
 
-  // Last thing to do: movement  
+  // Last thing to do: movement and update status vector
   drive->drivePrepared();  
+  updateStatusVector();
 } 

src/status_vector.cpp

@@ -0,0 +1,15 @@
+#define STATUS_VECTOR_CPP
+#include "status_vector.h"
+
+void initStatusVector(){
+    currentSVIndex = 0;
+
+    for(int i=0; i>=dim; i++){
+        inputs[i] = input();
+        datas[i] = data();
+    }
+}
+
+void updateStatusVector(){
+    currentSVIndex = (currentSVIndex+1) % dim;
+}

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (49, 99, -24, 17, 23, 64),    # thresholds yellow goal
-                (-128,-128,-128,-128,-128,-128)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+thresholds = [  (30, 70, -12, 19, 10, 57),    # thresholds yellow goal
+                (0, 31, -2, 39, -68, -25)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 

utility/OpenMV/main_test_conic.py

@@ -29,8 +29,13 @@ blue_led.on()
 #thresholds = [  (54, 93, -10, 25, 55, 70),    # thresholds yellow goal
 #                (30, 45, 1, 40, -60, -19)]    # thresholds blue goal
 #
+<<<<<<< HEAD
 thresholds = [  (49, 84, -8, 31, 20, 80),    # thresholds yellow goal
                 (0, 51, -4, 44, -59, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+=======
+thresholds = [  (30, 70, -12, 19, 41, 68)  ,    # thresholds yellow goal
+                (0, 70, -2, 34, -59, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf
 
 roi = (0, 6, 318, 152)
 
@@ -52,9 +57,13 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+2)
 sensor.set_saturation(+1)
+<<<<<<< HEAD
 sensor.set_brightness(-3)
+=======
+sensor.set_brightness(-2)
+>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 8000)
+sensor.set_auto_exposure(False, 6000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -79,7 +88,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=350, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=150, area_threshold=150, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())