First camera script for conic shaped mirror

include/drivecontroller.h

@@ -5,9 +5,9 @@
 
 //PID Constants
 
-#define KP 1.2    
+#define KP 0.9
 #define KI 0   
-#define KD 0.7 
+#define KD 0
 
 class DriveController{
 

src/goalie.cpp

@@ -69,7 +69,7 @@ void Goalie::storcimentoPorta() {
   if (camera->getValueAtk(true) >= 3) cstorc+=9;
   else if (camera->getValueAtk(true)  < -3) cstorc-=9;
   else cstorc *= 0.7;
-  cstorc = constrain(cstorc, -30, 30);
+  cstorc = constrain(cstorc, -45, 45);
 }
 
 void Goalie::ballBack() {

src/linesys_2019.cpp

@@ -77,7 +77,6 @@ void LineSys2019::outOfBounds(){
 
   if (exitTimer <= EXTIME){
     //fase di rientro
-    digitalWrite(LED_R, HIGH);
     if(linesens == 15) linesens = linesensOldY | linesensOldX;        //ZOZZATA MAXIMA
     unlockTime = millis();
 

utility/OpenMV/main_test_conic.py

@@ -0,0 +1,123 @@
+# 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 = [  (55, 98, -14, 12, 7, 55),    # thresholds yellow goal
+                (26, 65, -11, 47, -95, -36)]  # 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.QVGA)
+sensor.set_contrast(+0)
+sensor.set_saturation(+0)
+sensor.set_brightness(0)
+sensor.set_quality(0)
+sensor.set_auto_exposure(False, 8000)
+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=500, area_threshold=700
+
+
+
+
+
+, 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)+"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)+"b"
+    print (string_blue)   # test on serial terminal
+
+    #print ("..................................")
+
+print(clock.fps())