Now using arduino PID library for movements. There probably a better way to use this, but it works for now

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "lib/Arduino-PID-Library"]
+	path = lib/Arduino-PID-Library
+	url = https://github.com/br3ttb/Arduino-PID-Library/

include/drivecontroller.h

@@ -2,11 +2,12 @@
 
 #include <Arduino.h>
 #include "motor.h"
+#include "PID_v1.h"
 
 //PID Constants
-#define KP 2.1 
+#define KP 1.2
-#define KI 0   
+#define KI 0.0
-#define KD 0.05
+#define KD 0.25
 
 #define UNLOCK_THRESH 800
 
@@ -32,8 +33,11 @@ class DriveController{
         Motor* m2;
         Motor* m3;
         Motor* m4;
+        PID* pid;
         int pDir, pSpeed, pTilt;
-        float speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta;
+        int gDir, gSpeed, gTilt;
+        int speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta;
+        double input, output, setpoint;
         int vx, vy;
         
         float sins[360], cosins[360];

lib/Arduino-PID-Library

@@ -0,0 +1 @@
+Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb

src/drivecontroller.cpp

@@ -15,6 +15,9 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     pDir = 0;
     pSpeed = 0;
     pTilt = 0;
+    gDir = 0;
+    gSpeed = 0;
+    gTilt = 0;
 
     vx = 0;
     vy = 0;
@@ -24,14 +27,15 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     speed3 = 0;
     speed4 = 0;
 
+    pid = new PID(&input, &output, &setpoint, (double)KP,  (double)KI,  (double)KD, P_ON_M, REVERSE);
     delta = 0;
-    errorP = 0;
+    input = 0;
-    errorI = 0;
+    output = 0;
-    errorD = 0;
+    setpoint = 0;
-    
+
-    errorePre = 0;
+    pid->SetMode(AUTOMATIC);
-    pidfactor = 0;
+    pid->SetSampleTime(2);
-    integral = 0;
+
     canUnlock = true;
     unlockTime = 0;
 
@@ -50,6 +54,7 @@ void DriveController::prepareDrive(int dir, int speed, int tilt){
 void DriveController::prepareDrive(int dir, int speed){
     pDir = dir;
     pSpeed = speed;
+    pDir = gTilt;
 }
 
 void DriveController::drivePrepared(){
@@ -61,6 +66,10 @@ float DriveController::torad(float f){
 }
 
 void DriveController::drive(int dir, int speed, int tilt){
+    gDir = dir;
+    gSpeed = speed;
+    gTilt = tilt;
+
     vx = ((speed * cosins[dir]));
     vy = ((-speed * sins[dir]));
 
@@ -80,22 +89,21 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed4 = -(speed2);
 
     // calcola l'errore di posizione rispetto allo 0
-    delta = compass->getValue();
+    delta = (compass->getValue()-tilt+360)%360;
-    if(delta > 180) delta = delta-360;
+;
-    delta = delta - tilt;
+    setpoint = 0;
+    pid->SetControllerDirection(REVERSE);
 
-    // calcola correzione proporzionale
+    if(delta > 180) {
-    errorP = KP * delta;
+        setpoint = 359;//delta = delta-360;
+        pid->SetControllerDirection(DIRECT);
+    }
 
-    // calcola correzione derivativa
+    input = delta;
-    errorD = KD * (delta - errorePre);
-    errorePre = delta;
 
-    // calcola correzione integrativa
+    pid->Compute();             
-    integral = 0.5 * integral + delta;
+
-    errorI = KI * integral;
+    pidfactor = delta > 180 ? output*-1 : output;
-    // calcota correzione complessiva
-    pidfactor =  errorD + errorP + errorI;               
 
     speed1 += pidfactor;
     speed2 += pidfactor;

utility/OpenMV/main_test_conic.py.autosave

@@ -0,0 +1,117 @@
+# 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())