EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

status vector now working better. Still have to fix something

code_midgen
EmaMaker 2020-02-12 21:43:20 +01:00
parent fc840b84be
commit 8634041ecf
13 changed files with 70 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.vscode/extensions.json vendored
View File

@ -1,7 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

1
include/data_source_ball.h
View File

@ -10,5 +10,4 @@ class DataSourceBall : public DataSource{
int angle, distance;
bool ballSeen;
int dir, degrees2,b;
};

2
include/drivecontroller.h
View File

@ -7,7 +7,7 @@
//PID Constants
#define KP 1.2
#define KI 0.0
#define KD 0.25
#define KD 0.5
#define UNLOCK_THRESH 800

28
include/status_vector.h
View File

@ -1,7 +1,19 @@
#pragma once
#include <Arduino.h>
/**
* STATUS VECTOR:
* The status vector consists in two arrays of two different structs.
* One (inputs) holds the raw input read by the various sensors on the robot
* The other (datas) contains the useful data obtained by the eventual manipulation of the raw inputs
* This is made so that it ha an history of the inputs and datas if needed.
* This is an intermediator between all the classes representing the different components of the robot. It's preferable to not make the classes call one another
* All the data held by the structs in the status vector will be described here.
*
* REMEMBER: The value of a sensor in the status vector MUST be updated also if the sensor data didn't change
*
**/
#ifdef STATUS_VECTOR_CPP
#define sv_extr
#else
@ -9,22 +21,22 @@
#endif
#define dim 20
#define CURRENT_DATA_READ (datas[(currentSVIndex-1) % dim])
#define CURRENT_DATA_WRITE (datas[(currentSVIndex) % dim])
#define CURRENT_INPUT_READ (inputs[(currentSVIndex-1) % dim])
#define CURRENT_INPUT_WRITE (inputs[(currentSVIndex) % dim])
#define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)] )
#define CURRENT_DATA_WRITE ( datas[((currentSVIndex))] )
#define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] )
#define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] )
typedef struct input{
int IMUAngle, USfr, USsx, USdx, USrr, BT;
byte ballByte, cameraByte, lineByte;
byte ballByte, cameraByte, lineByte, xb, yb, xy, yy;
bool SW_DX, SW_SX;
}input;
typedef struct data{
int IMUAngle, ballAngle, ballDistance, cameraAngle, cameraDistance,
int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist,
speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role;
byte xb, yb, xy, yy, lineSeen, lineActive;
bool mate, ATKgoal, DEFgoal;
bool mate, ATKgoal, DEFgoal, ballSeen;
}data;
sv_extr input inputs[dim];

16
src/data_source_ball.cpp
View File

@ -8,12 +8,16 @@ DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(se
}
void DataSourceBall :: postProcess(){
if((value & 0x01) == 1){
distance = value;
ballSeen = distance > 1;
}else{
angle = value * 2;
}
if((value & 0x01) == 1){
distance = value;
ballSeen = distance > 1;
}else{
angle = value * 2;
}
CURRENT_INPUT_WRITE.ballByte = value;
CURRENT_DATA_WRITE.ballAngle = angle;
CURRENT_DATA_WRITE.ballDistance = distance;
CURRENT_DATA_WRITE.ballSeen = ballSeen;
}
void DataSourceBall :: test(){

6
src/data_source_bno055.cpp
View File

@ -17,10 +17,8 @@ void DataSourceBNO055::readSensor(){
if(millis() - lastTime > DATA_CLOCK){
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
this->value = (int) euler.x();
CURRENT_INPUT_WRITE.IMUAngle = this->value;
CURRENT_DATA_WRITE.IMUAngle = this->value;
lastTime = millis();
}
CURRENT_INPUT_WRITE.IMUAngle = this->value;
CURRENT_DATA_WRITE.IMUAngle = this->value;
}

14
src/data_source_camera_conicmirror.cpp
View File

@ -46,7 +46,7 @@ void DataSourceCameraConic :: readSensor(){
yAngle = (yAngle+360)%360;
bAngle = (bAngle+360)%360;
int angleFix = compass->getValue() > 180 ? compass->getValue() - 360 : compass->getValue();
int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
//Fixes with IMU
yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ;
@ -66,6 +66,18 @@ void DataSourceCameraConic :: readSensor(){
count++;
}
}
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.xb = true_xb;
CURRENT_DATA_WRITE.yb = true_yb;
CURRENT_DATA_WRITE.xy = true_xy;
CURRENT_DATA_WRITE.yy = true_yy;
CURRENT_DATA_WRITE.yAngle = yAngle;
CURRENT_DATA_WRITE.bAngle = bAngle;
CURRENT_DATA_WRITE.yAngleFix = yAngleFix;
CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
CURRENT_DATA_WRITE.yDist = yDist;
CURRENT_DATA_WRITE.bDist = bDist;
}
}

5
src/drivecontroller.cpp
View File

@ -76,8 +76,9 @@ void DriveController::drive(int dir, int speed, int tilt){
speed4 = -(speed2);
// calcola l'errore di posizione rispetto allo 0
delta = (compass->getValue()-tilt+360)%360;
;
// delta = (compass->getValue()-tilt+360)%360;
delta = (CURRENT_DATA_READ.IMUAngle-tilt+360)%360;
setpoint = 0;
pid->SetControllerDirection(REVERSE);

18
src/goalie.cpp
View File

@ -21,24 +21,24 @@ void Goalie::realPlay(){
else drive->prepareDrive(0,0,0);
}
int dir, degrees2;
void Goalie::goalie(int plusang) {
if(ball->distance < 185) drive->prepareDrive(ball->angle, 350, 0);
else{
if(ball->angle > 340 || ball->angle < 20) plusang -= 20;
if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
else ball->degrees2 = ball->angle;
if(ball->angle > 180) degrees2 = ball->angle - 360;
else degrees2 = ball->angle;
if(ball->degrees2 > 0) ball->dir = ball->angle + plusang; //45 con 8 ruote
else ball->dir = ball->angle - plusang; //45 con 8 ruote
if(degrees2 > 0) dir = ball->angle + plusang; //45 con 8 ruote
else dir = ball->angle - plusang; //45 con 8 ruote
if(ball->dir < 0) ball->dir = ball->dir + 360;
else ball->dir = ball->dir;
ball->b = ball->dir;
if(dir < 0) dir = dir + 360;
else dir = 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(dir, 350, cstorc);
else {
drive->prepareDrive(ball->dir, 350, 0);
drive->prepareDrive(dir, 350, 0);
cstorc = 0;
}
}

2
src/main.cpp
View File

@ -22,6 +22,8 @@ void loop() {
goalie->play(role==1);
keeper->play(role==0);
Serial.println(CURRENT_DATA_READ.IMUAngle);
// Last thing to do: movement and update status vector
drive->drivePrepared();
updateStatusVector();

2
src/status_vector.cpp
View File

@ -4,7 +4,7 @@
void initStatusVector(){
currentSVIndex = 0;
for(int i=0; i>=dim; i++){
for(int i=0; i<dim; i++){
inputs[i] = input();
datas[i] = data();
}

13
utility/OpenMV/main_test_conic.py
View File

@ -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)
@ -84,11 +75,11 @@ while(True):
blue_led.off()
tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
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=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())

117
utility/OpenMV/main_test_conic.py.autosave
View File

@ -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())