camera working
EmaMaker
parent
00f6192255
commit
9afd6bf8b6
|
|
@ -2,8 +2,12 @@
|
|||
#include "systems.h"
|
||||
|
||||
#define CAMERA_CENTER_X 0
|
||||
#define CAMERA_CENTER_Y -13
|
||||
#define CAMERA_CENTER_Y -18
|
||||
#define CAMERA_CENTER_Y_ABS_SUM 72
|
||||
//Actually it's ± MAX_VAL
|
||||
#define MAX_X 25
|
||||
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
|
||||
#define DIST_MULT 1.65
|
||||
|
||||
#define Kpx 1
|
||||
#define Kix 0
|
||||
|
|
@ -24,7 +28,7 @@ class PositionSysCamera : public PositionSystem{
|
|||
int calcOtherGoalY(int goalY);
|
||||
|
||||
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
|
||||
|
||||
int MAX_DIST;
|
||||
PID* X;
|
||||
PID* Y;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
#pragma once
|
||||
#define DEBUG Serial3
|
||||
#define DEBUG Serial
|
||||
|
||||
#define GLOBAL_SPD_MULT 1.0
|
||||
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ void PositionSysCamera::goCenter(){
|
|||
/*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
|
||||
/* int x = 1;
|
||||
int y = 1;
|
||||
|
||||
;
|
||||
//Trying using an angle
|
||||
if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
|
||||
if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y)
|
||||
|
|
@ -44,6 +44,7 @@ void PositionSysCamera::goCenter(){
|
|||
|
||||
//using a pid controller for the movement, or trying at least
|
||||
void PositionSysCamera :: setCameraPID(){
|
||||
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
|
||||
Inputx = 0;
|
||||
Outputx = 0;
|
||||
Setpointx = 0;
|
||||
|
|
@ -65,7 +66,7 @@ void PositionSysCamera :: setCameraPID(){
|
|||
|
||||
/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one
|
||||
We know the sum of the absolute values is a fixed number.
|
||||
By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
|
||||
By♦ subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
|
||||
The sign of the goal y we found is simply the reverse of the one we got
|
||||
*/
|
||||
int PositionSysCamera::calcOtherGoalY(int goalY){
|
||||
|
|
@ -86,11 +87,10 @@ void PositionSysCamera :: CameraPID(){
|
|||
Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
|
||||
//Setpointy todo
|
||||
}else{
|
||||
|
||||
//TODO: no goal seen
|
||||
}
|
||||
Setpointx = CAMERA_CENTER_X;
|
||||
Setpointy = CAMERA_CENTER_Y;
|
||||
//TODO: no goal seen
|
||||
|
||||
X->Compute();
|
||||
Y->Compute();
|
||||
|
|
@ -99,11 +99,15 @@ void PositionSysCamera :: CameraPID(){
|
|||
// DEBUG.print(" ");
|
||||
// DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
|
||||
|
||||
if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
|
||||
/*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
|
||||
drive->prepareDrive(0,0,0);
|
||||
}else{
|
||||
}else{*/
|
||||
int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14);
|
||||
dir = (dir+360) % 360;
|
||||
drive->prepareDrive(dir, 100, 0);
|
||||
}
|
||||
|
||||
int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
|
||||
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
|
||||
drive->prepareDrive(dir, speed, 0);
|
||||
//DEBUG.println(dir);
|
||||
//}
|
||||
}
|
||||
|
|
@ -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 = [ (21, 100, -53, 53, 59, 101), # thresholds yellow goal
|
||||
(29, 50, -48, 35, -56, 5)] # 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(2)
|
||||
sensor.set_saturation(1)
|
||||
sensor.set_brightness(1)
|
||||
sensor.set_quality(3)
|
||||
sensor.set_auto_whitebal(False)
|
||||
sensor.set_auto_exposure(False, 4500)
|
||||
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=60, area_threshold=90, 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)
|
||||
|
||||
Loading…
Reference in New Issue