not working

include/goalie.h

@@ -16,7 +16,7 @@
 #define GOALIE_ATKDIR_PLUSANG2_COR 70
 #define GOALIE_ATKDIR_PLUSANG3_COR 70
 
-class Goalie : public Game{
+class Goalie : public Game, public PositionSysZone{
 
     public:
         Goalie();

include/positionsys_zone.h

@@ -60,6 +60,7 @@ class PositionSysZone : public PositionSystem{
         PositionSysZone();
         void update() override;
         void test() override;
+        void goCenter();
 
         //returns the zone calculated
         int getValue();
@@ -110,7 +111,7 @@ class PositionSysZone : public PositionSystem{
         void testLogicZone();
 
         //movement
-        void goCenter();
+        //void goCenter();
         void goGoalPost();
         void centerGoalPost();
         void centerGoalPostCamera(bool);

src/drivecontroller.cpp

@@ -31,7 +31,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     setpoint = 0;
 
     pid->SetMode(AUTOMATIC);
-    pid->SetSampleTime(2);
+    pid->SetSampleTime(5);
 
     canUnlock = true;
     unlockTime = 0;

src/goalie.cpp

@@ -1,6 +1,7 @@
 #include "goalie.h"
 #include "sensors.h"
 #include "vars.h"
+#include "positionsys_zone.h"
 
 Goalie::Goalie() : Game() {
   init();
@@ -17,7 +18,7 @@ void Goalie::init(){
 }
 
 void Goalie::realPlay(){
-  if(ball->ballSeen) this->goalie(50);
+  if(ball->ballSeen) this->goCenter();
   else drive->prepareDrive(0,0,0);
 }
 
@@ -36,11 +37,11 @@ void Goalie::goalie(int plusang) {
     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(ball->dir, 350, cstorc);
-    else {
+    //else {
       drive->prepareDrive(ball->dir, 350, 0);
-      cstorc = 0;
+    //  cstorc = 0;
-    }
+    //}
   }
 }
 

src/positionsys_zone.cpp

@@ -373,7 +373,15 @@ void PositionSysZone::testLogicZone(){
 
 
 void PositionSysZone::goCenter() {
-  if (zoneIndex == 8)
+  if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) {
+    //dx o sx
+   if((camera->true_yb + camera->true_yy) < 0) drive->prepareDrive(0, 200, 0);
+   else if ((camera->true_yb + camera->true_yy) > 0) drive->prepareDrive(270, 200, 0);
+  }  
+  if(camera->true_xb >= -10 && camera->true_xb <= 10);
+  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);
+ /*  if (zoneIndex == 8)
     drive->prepareDrive(330, GOCENTER_VEL);
   if (zoneIndex == 7)
     drive->prepareDrive(0, GOCENTER_VEL);
@@ -390,7 +398,7 @@ void PositionSysZone::goCenter() {
   if (zoneIndex == 1)
     drive->prepareDrive(180, GOCENTER_VEL);
   if (zoneIndex == 0)
-    drive->prepareDrive(135, GOCENTER_VEL);
+    drive->prepareDrive(135, GOCENTER_VEL); */
 }
 
 

utility/OpenMV/conic_eff.py

@@ -40,7 +40,7 @@ blue_led.on()
 
 
 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)
+                (0, 44, -5, 42, -65, -13)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -62,7 +62,7 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
 sensor.set_saturation(0)
-sensor.set_brightness(0)
+sensor.set_brightness(-2)
 sensor.set_quality(0)
 sensor.set_auto_exposure(False, 10000)
 sensor.set_auto_gain(True)

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)
@@ -88,7 +79,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=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())