Now using Flavio's PID Library, plays flawlessly

include/positionsys_zone.h

@@ -55,6 +55,9 @@
 #define SOUTH_CENTER 8
 #define SOUTH_EAST 9
 
+#define CAMERA_CENTER_X 3
+#define CAMERA_CENTER_Y 10
+
 class PositionSysZone : public PositionSystem{
     public:
         PositionSysZone();

include/vars.h

@@ -1,5 +1,5 @@
 #pragma once
-#define DEBUG Serial3
+#define DEBUG Serial
 
 #define LED_R 20
 #define LED_Y 17

src/goalie.cpp

@@ -18,7 +18,7 @@ void Goalie::init(){
 }
 
 void Goalie::realPlay(){
-  if(ball->ballSeen) this->goalie(50);
+  if(ball->ballSeen) this->goalie(45);
   else ((PositionSysZone*)ps)->goCenter();
 }
 

src/main.cpp

@@ -3,6 +3,7 @@
 #include "sensors.h"
 #include "games.h"
 #include "status_vector.h"
+#include "positionsys_zone.h"
 
 void setup() {
   delay(1500);
@@ -22,7 +23,11 @@ void loop() {
   goalie->play(role==1);
   keeper->play(role==0);  
 
+  camera->test();
+
+
   // Last thing to do: movement and update status vector
+
   drive->drivePrepared();  
   updateStatusVector();
 } 

src/positionsys_zone.cpp

@@ -373,15 +373,15 @@ void PositionSysZone::testLogicZone(){
 
 
 void PositionSysZone::goCenter() {
-  if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) {
+  if((camera->true_yb + camera->true_yy) >= 0) drive->prepareDrive(180, 75, 0);
-    //dx o sx
+  else if ((camera->true_yb + camera->true_yy) <= -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); 
-   if((camera->true_yb + camera->true_yy) < 0) drive->prepareDrive(0, 200, 0);
+  else drive->prepareDrive(0, 0, 0);
-   else if ((camera->true_yb + camera->true_yy) > 0) drive->prepareDrive(270, 200, 0);
+/*   if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
-  }  
+  else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
-  if(camera->true_xb >= -10 && camera->true_xb <= 10);
+  else drive->prepareDrive(0, 0, 0); */
-  else if(camera->true_xb >= -5 && camera->true_xb <= 35) drive->prepareDrive(90, 200, 0);
+ /*  
-  else if(camera->true_xb >= 5 && camera->true_xb <= -35) drive->prepareDrive(180, 200, 0);
+ PREVIOUS
- /*  if (zoneIndex == 8)
+  if (zoneIndex == 8)
     drive->prepareDrive(330, GOCENTER_VEL);
   if (zoneIndex == 7)
     drive->prepareDrive(0, GOCENTER_VEL);

utility/OpenMV/conic_eff.py.autosave

@@ -0,0 +1,149 @@
+# 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)
+
+START_BYTE = chr(105) #'i'
+END_BYTE = chr(115) #'s'
+BYTE_UNKNOWN = chr(116) #'t'
+
+y_found = False
+b_found = False
+
+#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
+def val_map(x, in_min, in_max, out_min, out_max):
+    x = int(x)
+    in_min = int(in_min)
+    in_max = int(in_max)
+    out_min = int(out_min)
+    out_max = int(out_max)
+    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
+
+
+# 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, 70, -12, 19, 10, 57),    # thresholds yellow goal
+                (0, 44, -5, 42, -65, -13)]  # 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(+3)
+sensor.set_saturation(0)
+sensor.set_brightness(-2)
+sensor.set_quality(0)
+sensor.set_auto_exposure(False, 10000)
+sensor.set_auto_gain(True)
+sensor.skip_frames(time = 300)
+
+clock = time.clock()
+##############################################################################
+
+
+while(True):
+    clock.tick()
+
+    blue_led.off()
+
+    y_found = False
+    b_found = False
+
+    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=75, area_threshold=130, 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() ) ]
+            y_found = True
+        if (blob.code() == 2):
+            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
+            b_found = True
+
+    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)
+
+    y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
+    b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1]
+
+    y_cx = int(img.width() / 2 - y1_cx)
+    y_cy = int(img.height() / 2 - y1_cy)
+    b_cx = int(img.width() / 2 - b1_cx)
+    b_cy = int(img.height() / 2 - b1_cy)
+
+    #Normalize data between 0 and 100
+    if y_found == True:
+        y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 100, 0)
+        y_cy = val_map(y_cy, -img.height() / 2, img.height() / 2, 0, 100)
+        #Prepare for send as a list of characters
+        s_ycx = chr(y_cx)
+        s_ycy = chr(y_cy)
+    else:
+        y_cx = BYTE_UNKNOWN
+        y_cy = BYTE_UNKNOWN
+        #Prepare for send as a list of characters
+        s_ycx = y_cx
+        s_ycy = y_cy
+
+    if b_found == True:
+        b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 100, 0)
+        b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
+
+        #Prepare for send as a list of characters
+        s_bcx = chr(b_cx)
+        s_bcy = chr(b_cy)
+    else:
+        b_cx = BYTE_UNKNOWN
+        b_cy = BYTE_UNKNOWN
+        #Prepare for send as a list of characters
+        s_bcx = b_cx
+        s_bcy = b_cy
+
+    print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
+
+
+    uart.write(START_BYTE)
+    uart.write(s_bcx)
+    uart.write(s_bcy)
+    uart.write(s_ycx)
+    uart.write(s_ycy)
+    uart.write(END_BYTE)
+