Now using arduino PID library for movements. There probably a better way to use this, but it works for now
EmaMaker
parent
1b2a810ad3
commit
2577cd568c
|
|
@ -0,0 +1,3 @@
|
|||
[submodule "lib/Arduino-PID-Library"]
|
||||
path = lib/Arduino-PID-Library
|
||||
url = https://github.com/br3ttb/Arduino-PID-Library/
|
||||
|
|
@ -2,11 +2,12 @@
|
|||
|
||||
#include <Arduino.h>
|
||||
#include "motor.h"
|
||||
#include "PID_v1.h"
|
||||
|
||||
//PID Constants
|
||||
#define KP 2.1
|
||||
#define KI 0
|
||||
#define KD 0.05
|
||||
#define KP 1.2
|
||||
#define KI 0.0
|
||||
#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];
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb
|
||||
|
|
@ -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;
|
||||
errorI = 0;
|
||||
errorD = 0;
|
||||
input = 0;
|
||||
output = 0;
|
||||
setpoint = 0;
|
||||
|
||||
pid->SetMode(AUTOMATIC);
|
||||
pid->SetSampleTime(2);
|
||||
|
||||
errorePre = 0;
|
||||
pidfactor = 0;
|
||||
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();
|
||||
if(delta > 180) delta = delta-360;
|
||||
delta = delta - tilt;
|
||||
delta = (compass->getValue()-tilt+360)%360;
|
||||
;
|
||||
setpoint = 0;
|
||||
pid->SetControllerDirection(REVERSE);
|
||||
|
||||
// calcola correzione proporzionale
|
||||
errorP = KP * delta;
|
||||
if(delta > 180) {
|
||||
setpoint = 359;//delta = delta-360;
|
||||
pid->SetControllerDirection(DIRECT);
|
||||
}
|
||||
|
||||
// calcola correzione derivativa
|
||||
errorD = KD * (delta - errorePre);
|
||||
errorePre = delta;
|
||||
input = delta;
|
||||
|
||||
// calcola correzione integrativa
|
||||
integral = 0.5 * integral + delta;
|
||||
errorI = KI * integral;
|
||||
// calcota correzione complessiva
|
||||
pidfactor = errorD + errorP + errorI;
|
||||
pid->Compute();
|
||||
|
||||
pidfactor = delta > 180 ? output*-1 : output;
|
||||
|
||||
speed1 += pidfactor;
|
||||
speed2 += pidfactor;
|
||||
|
|
|
|||
|
|
@ -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())
|
||||
Loading…
Reference in New Issue