EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

camera pid testing

pull/1/head
u-siri-ous 2020-02-26 18:51:46 +01:00
parent e0c7c569f6
commit 3c34a71676
7 changed files with 233 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/positionsys_camera.h
View File

@ -1,7 +1,15 @@
#include "PID_v2.h"
#include "systems.h" #include "systems.h"
#define CAMERA_CENTER_X 3 #define CAMERA_CENTER_X 3
#define CAMERA_CENTER_Y 6 #define CAMERA_CENTER_Y 3
#define Kpx 1
#define Kix 0
#define Kdx 0
#define Kpy 1
#define Kiy 0
#define Kdy 0
class PositionSysCamera : public PositionSystem{ class PositionSysCamera : public PositionSystem{
@ -10,5 +18,12 @@ class PositionSysCamera : public PositionSystem{
void goCenter(); void goCenter();
void update() override; void update() override;
void test() override; void test() override;
void setCameraPID();
void CameraPID();
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
PID* X;
PID* Y;
}; };

1
lib/Arduino-PID-Library Submodule

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

2
src/drivecontroller.cpp
View File

@ -47,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
vyn = 0; vyn = 0;
} }
void DriveController::prepareDrive(int dir, int speed, int tilt=0){ void DriveController::prepareDrive(int dir, int speed, int tilt){
pDir = dir; pDir = dir;
pSpeed = speed; pSpeed = speed;
pTilt = tilt; pTilt = tilt;

1
src/main.cpp
View File

@ -25,6 +25,7 @@ void loop() {
// Last thing to do: movement and update status vector // Last thing to do: movement and update status vector
// drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix); // drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix);
drive->drivePrepared(); drive->drivePrepared();
updateStatusVector(); updateStatusVector();
} }

72
src/positionsys_camera.cpp
View File

@ -3,7 +3,9 @@
#include "vars.h" #include "vars.h"
#include "sensors.h" #include "sensors.h"
PositionSysCamera::PositionSysCamera() {} PositionSysCamera::PositionSysCamera() {
setCameraPID();
}
void PositionSysCamera::update(){ void PositionSysCamera::update(){
} }
@ -14,26 +16,70 @@ void PositionSysCamera::test(){
void PositionSysCamera::goCenter(){ void PositionSysCamera::goCenter(){
/*WORKS BUT CAN BE BETTER*/ /*WORKS BUT CAN BE BETTER*/
//Y //Y
if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
//X //X
else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
else drive->prepareDrive(0, 0, 0); else drive->prepareDrive(0, 0, 0); */
/*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/ /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
// int x = 1; /* int x = 1;
// int y = 1; int y = 1;
// //Trying using an angle //Trying using an angle
// if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
// y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y)
// if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
// if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
// int dir = -90-(atan2(y*1.5,x)*180/3.14); int dir = -90-(atan2(y*1.5,x)*180/3.14);
// dir = (dir+360) % 360; dir = (dir+360) % 360;
// drive->prepareDrive(dir, 100, 0); drive->prepareDrive(dir, 100, 0);
} */
CameraPID();
}
//using a pid controller for the movement, or trying at least
void PositionSysCamera :: setCameraPID(){
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
X->SetOutputLimits(-50,50);
X->SetMode(AUTOMATIC);
X->SetDerivativeLag(1);
X->SetControllerDirection(DIRECT);
X->SetSampleTime(1.5);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE);
Y->SetOutputLimits(-50,50);
Y->SetMode(AUTOMATIC);
Y->SetDerivativeLag(1);
Y->SetControllerDirection(DIRECT);
Y->SetSampleTime(1.5);
}
void PositionSysCamera :: CameraPID(){
if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2;
Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
Setpointx = 0;
Setpointy = 0;
}
if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
Inputx = CURRENT_DATA_READ.cam_xb;
Inputy = CURRENT_DATA_READ.cam_yb;
}
if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputx = CURRENT_DATA_READ.cam_xy;
Inputy = CURRENT_DATA_READ.cam_yy;
//Setpointy todo
}
//TODO: no goal seen
X->Compute();
Y->Compute();
int dir = -90-(atan2(Outputy*1.5,Outputx)*180/3.14);
dir = (dir+360) % 360;
if(Outputx > 0) drive->prepareDrive(90, 100, 0);
else drive->prepareDrive(270, 100, 0);
} }

11
utility/OpenMV/conic_eff.py
View File

@ -38,8 +38,8 @@ blue_led.on()
############################################################################## ##############################################################################
thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal thresholds = [ (45, 71, 0, 20, 56, 84) , # thresholds yellow goal
(39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (16, 27, -29, -7, -10, 11)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)
@ -60,10 +60,11 @@ sensor.reset()
sensor.set_pixformat(sensor.RGB565) sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3) sensor.set_contrast(+3)
sensor.set_saturation(0) sensor.set_saturation(2)
sensor.set_brightness(-2) sensor.set_brightness(0)
sensor.set_quality(0) sensor.set_quality(0)
sensor.set_auto_exposure(False, 6000) sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 2000)
sensor.set_auto_gain(True) sensor.set_auto_gain(True)
sensor.skip_frames(time = 300) sensor.skip_frames(time = 300)

149
utility/OpenMV/conic_eff.py.autosave Normal file
View File

@ -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 = [ (40, 98, -26, 2, 68, 110) , # thresholds yellow goal
(26, 100, -38, -4, -20, 16)] # 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) vbc
#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(2)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 6000)
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=50, area_threshold=80, 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)