status vector now working better. Still have to fix something

.vscode/extensions.json

@@ -1,7 +1,7 @@
 {
-	// See http://go.microsoft.com/fwlink/?LinkId=827846
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
-	// for the documentation about the extensions.json format
+    // for the documentation about the extensions.json format
-	"recommendations": [
+    "recommendations": [
-		"platformio.platformio-ide"
+        "platformio.platformio-ide"
-	]
+    ]
 }

include/data_source_ball.h

@@ -10,5 +10,4 @@ class DataSourceBall : public DataSource{
         
         int angle, distance;
         bool ballSeen;
-        int dir, degrees2,b;
 }; 

include/drivecontroller.h

@@ -7,7 +7,7 @@
 //PID Constants
 #define KP 1.2
 #define KI 0.0
-#define KD 0.25
+#define KD 0.5
 
 #define UNLOCK_THRESH 800
 

include/status_vector.h

@@ -1,7 +1,19 @@
 #pragma once
-
 #include <Arduino.h>
 
+/**
+ * STATUS VECTOR:
+ * The status vector consists in two arrays of two different structs.
+ * One (inputs) holds the raw input read by the various sensors on the robot
+ * The other (datas) contains the useful data obtained by the eventual manipulation of the raw inputs
+ * This is made so that it ha an history of the inputs and datas if needed.
+ * This is an intermediator between all the classes representing the different components of the robot. It's preferable to not make the classes call one another
+ * All the data held by the structs in the status vector will be described here.
+ * 
+ * REMEMBER: The value of a sensor in the status vector MUST be updated also if the sensor data didn't change
+ * 
+**/
+
 #ifdef STATUS_VECTOR_CPP
 #define sv_extr 
 #else
@@ -9,22 +21,22 @@
 #endif
 
 #define dim 20
-#define CURRENT_DATA_READ (datas[(currentSVIndex-1) % dim])
+#define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)]  )
-#define CURRENT_DATA_WRITE (datas[(currentSVIndex) % dim])
+#define CURRENT_DATA_WRITE ( datas[((currentSVIndex))]  )
-#define CURRENT_INPUT_READ (inputs[(currentSVIndex-1) % dim])
+#define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] )
-#define CURRENT_INPUT_WRITE (inputs[(currentSVIndex) % dim])
+#define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] )
 
 typedef struct input{
     int IMUAngle, USfr, USsx, USdx, USrr, BT;
-    byte ballByte, cameraByte, lineByte;
+    byte ballByte, cameraByte, lineByte, xb, yb, xy, yy;
     bool SW_DX, SW_SX;
 }input;
 
 typedef struct data{
-    int IMUAngle, ballAngle, ballDistance, cameraAngle, cameraDistance,
+    int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist,
         speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role;
     byte xb, yb, xy, yy, lineSeen, lineActive;
-    bool mate, ATKgoal, DEFgoal;
+    bool mate, ATKgoal, DEFgoal, ballSeen;
 }data;
 
 sv_extr input inputs[dim];

src/data_source_ball.cpp

@@ -8,12 +8,16 @@ DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(se
 }
 
 void DataSourceBall :: postProcess(){
-    if((value & 0x01) == 1){
+  if((value & 0x01) == 1){
-        distance = value;
+      distance = value;
-        ballSeen = distance > 1;
+      ballSeen = distance > 1;
-      }else{ 
+  }else{ 
-        angle = value * 2; 
+    angle = value * 2; 
-      }
+  }
+  CURRENT_INPUT_WRITE.ballByte = value;
+  CURRENT_DATA_WRITE.ballAngle = angle;
+  CURRENT_DATA_WRITE.ballDistance = distance;
+  CURRENT_DATA_WRITE.ballSeen = ballSeen;
 }
 
 void DataSourceBall :: test(){

src/data_source_bno055.cpp

@@ -17,10 +17,8 @@ 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();
   }
+  CURRENT_INPUT_WRITE.IMUAngle = this->value;
+  CURRENT_DATA_WRITE.IMUAngle = this->value;
 }

src/data_source_camera_conicmirror.cpp

@@ -46,7 +46,7 @@ void DataSourceCameraConic :: readSensor(){
         yAngle = (yAngle+360)%360;
         bAngle = (bAngle+360)%360;
 
-        int angleFix = compass->getValue() > 180 ? compass->getValue() - 360 : compass->getValue();
+        int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
 
         //Fixes with IMU
         yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ;
@@ -66,6 +66,18 @@ void DataSourceCameraConic :: readSensor(){
         count++;
       }
     }   
+    //Important: update status vector
+    CURRENT_INPUT_WRITE.cameraByte = value;
+    CURRENT_DATA_WRITE.xb = true_xb;
+    CURRENT_DATA_WRITE.yb = true_yb;
+    CURRENT_DATA_WRITE.xy = true_xy;
+    CURRENT_DATA_WRITE.yy = true_yy;
+    CURRENT_DATA_WRITE.yAngle = yAngle;
+    CURRENT_DATA_WRITE.bAngle = bAngle;
+    CURRENT_DATA_WRITE.yAngleFix = yAngleFix;
+    CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
+    CURRENT_DATA_WRITE.yDist = yDist;
+    CURRENT_DATA_WRITE.bDist = bDist;
   }
 }
 

src/drivecontroller.cpp

@@ -76,8 +76,9 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed4 = -(speed2);
 
     // calcola l'errore di posizione rispetto allo 0
-    delta = (compass->getValue()-tilt+360)%360;
+//    delta = (compass->getValue()-tilt+360)%360;
-;
+   delta = (CURRENT_DATA_READ.IMUAngle-tilt+360)%360;
+
     setpoint = 0;
     pid->SetControllerDirection(REVERSE);
 

src/goalie.cpp

@@ -21,24 +21,24 @@ void Goalie::realPlay(){
   else drive->prepareDrive(0,0,0);
 }
 
+int dir, degrees2;
 void Goalie::goalie(int plusang) {
   if(ball->distance < 185) drive->prepareDrive(ball->angle, 350, 0);
   else{
     if(ball->angle > 340 || ball->angle < 20) plusang -= 20;
-    if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
+    if(ball->angle > 180) degrees2 = ball->angle - 360;
-    else ball->degrees2 = ball->angle;
+    else degrees2 = ball->angle;
 
-    if(ball->degrees2 > 0) ball->dir = ball->angle + plusang;              //45 con 8 ruote
+    if(degrees2 > 0) dir = ball->angle + plusang;              //45 con 8 ruote
-    else ball->dir = ball->angle - plusang;                                 //45 con 8 ruote
+    else dir = ball->angle - plusang;                                 //45 con 8 ruote
 
-    if(ball->dir < 0) ball->dir = ball->dir + 360;
+    if(dir < 0) dir = dir + 360;
-    else ball->dir = ball->dir;
+    else dir = dir;
-    ball->b = ball->dir;
 
     storcimentoPorta();
-    if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc);
+    if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
     else {
-      drive->prepareDrive(ball->dir, 350, 0);
+      drive->prepareDrive(dir, 350, 0);
       cstorc = 0;
     }
   }

src/main.cpp

@@ -22,6 +22,8 @@ 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

@@ -4,7 +4,7 @@
 void initStatusVector(){
     currentSVIndex = 0;
 
-    for(int i=0; i>=dim; i++){
+    for(int i=0; i<dim; i++){
         inputs[i] = input();
         datas[i] = data();
     }

utility/OpenMV/main_test_conic.py

@@ -29,13 +29,8 @@ 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)
 
@@ -57,11 +52,7 @@ 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, 6000)
 sensor.set_auto_gain(True)
@@ -84,11 +75,11 @@ while(True):
 
     blue_led.off()
 
-    tt_yellow = [(0,999,0,1)]     ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
+    tt_yellow = [(0,999,0,1)]      ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
     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=150, area_threshold=150, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=150, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())
 

utility/OpenMV/main_test_conic.py.autosave

@@ -1,117 +0,0 @@
-# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
-# Based on:
-# color tracking - By: paolix - ven mag 18 2018
-
-# Automatic RGB565 Color Tracking Example
-#
-import sensor, image, time, pyb, math
-
-from pyb import UART
-uart = UART(3,19200, timeout_char = 1000)
-
-
-# LED Setup ##################################################################
-
-red_led = pyb.LED(1)
-green_led = pyb.LED(2)
-blue_led = pyb.LED(3)
-
-red_led.off()
-green_led.off()
-blue_led.on()
-##############################################################################
-
-
-#thresholds = [  (30, 100, 15, 127, 15, 127),    # generic_red_thresholds
-#                (30, 100, -64, -8, -32, 32),    # generic_green_thresholds
-#                (0, 15, 0, 40, -80, -20)]       # generic_blue_thresholds
-
-#thresholds = [  (54, 93, -10, 25, 55, 70),    # thresholds yellow goal
-#                (30, 45, 1, 40, -60, -19)]    # thresholds blue goal
-#
-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)
-
-roi = (0, 6, 318, 152)
-
-# Camera Setup ###############################################################
-'''sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QVGA)
-sensor.skip_frames(time = 2000)
-sensor.set_auto_gain(False)         # must be turned off for color tracking
-sensor.set_auto_whitebal(False)     # must be turned off for color tracking
-sensor.set_auto_exposure(False, 10000)
-#sensor.set_backlight(1)
-#sensor.set_brightness(+2)
-#sensor.set_windowing(roi)
-clock = time.clock()'''
-
-sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QQVGA)
-sensor.set_contrast(+2)
-sensor.set_saturation(+1)
-sensor.set_brightness(-3)
-sensor.set_quality(0)
-sensor.set_auto_exposure(False, 6000)
-sensor.set_auto_gain(True)
-sensor.skip_frames(time = 300)
-
-clock = time.clock()
-##############################################################################
-
-
-
-# [] list
-# () tupla
-
-'''while(True):
-    clock.tick()
-    img = sensor.snapshot()'''
-
-while(True):
-    clock.tick()
-
-    blue_led.off()
-
-    tt_yellow = [(0,999,0,1)]      ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
-    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=150, merge = True):
-        img.draw_rectangle(blob.rect())
-        img.draw_cross(blob.cx(), blob.cy())
-
-        if (blob.code() == 1):
-            tt_yellow = tt_yellow +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-        if (blob.code() == 2):
-            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-
-    tt_yellow.sort(key=lambda tup: tup[0])  ## ordino le liste
-    tt_blue.sort(key=lambda tup: tup[0])    ## ordino le liste
-
-    ny = len(tt_yellow)
-    nb = len(tt_blue)
-
-    '''Yellow'''
-    area,cx,cy,code = tt_yellow[ny-1]    # coordinata x del piu' grande y se montata al contrario
-    cx = img.width() / 2 - cx
-    cy = img.height() / 2 - cy
-    angle = math.pi/2 - math.atan2(cy, cx)
-    dist = math.sqrt(cx*cx + cy*cy)
-    string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(angle)+" | "+str(dist)+str(area)+"y"
-    print (string_yellow)   # test on serial terminal
-
-    '''Blue'''
-    area,cx,cy,code = tt_blue[nb-1]    # coordinata x del piu' grande y se montata al contrario
-    cx = img.width() / 2 - cx
-    cy = img.height() / 2 - cy
-    angle = math.pi/2 - math.atan2(cy, cx)
-    dist = math.sqrt(cx*cx + cy*cy)
-    string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(angle)+" | "+str(dist)+str(area)+"b"
-    print (string_blue)   # test on serial terminal
-
-    #print ("..................................")
-
-print(clock.fps())