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

Merge branch 'master' into newcode

docs
EmaMaker 2020-11-23 16:54:50 +01:00
parent 2d573211a5 2354015201
commit b17e54ae27
30 changed files with 635 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/behaviour_control/status_vector.h
View File

@ -1,7 +1,7 @@
#pragma once
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "position/systems.h"
#include "systems/systems.h"
/**
* STATUS VECTOR:
@ -43,7 +43,8 @@ typedef struct data{
speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr,
lineOutDir, matePos, role, cam_xb_fixed,
cam_xy_fixed, cam_yb_fixed, cam_yy_fixed;
cam_xy_fixed, cam_yb_fixed, cam_yy_fixed,
posx, posy, addvx, addvy;
Game* game;
LineSystem* lineSystem;
PositionSystem* posSystem;

2
include/motors_movement/drivecontroller.h
View File

@ -51,6 +51,4 @@ class DriveController{
double input, output, setpoint;
float sins[360], cosins[360];
};

8
include/sensors/sensors.h
View File

@ -12,10 +12,10 @@
#include "behaviour_control/ds_ctrl.h"
#include "motors_movement/drivecontroller.h"
#include "motors_movement/motor.h"
#include "sensors/linesys_2019.h"
#include "sensors/linesys_camera.h"
#include "position/positionsys_zone.h"
#include "position/systems.h"
#include "systems/lines/linesys_2019.h"
#include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_zone.h"
#include "systems/systems.h"
#include "sensors/data_source_ball.h"
#include "sensors/data_source_bt.h"
#include "sensors/data_source_bno055.h"

2
include/strategy_roles/game.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "vars.h"
#include "position/systems.h"
#include "systems/systems.h"
#include "sensors/sensors.h"
class Game {

4
include/strategy_roles/games.h
View File

@ -8,10 +8,10 @@
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "strategy_roles/goalie.h"
#include "strategy_roles/striker.h"
#include "strategy_roles/keeper.h"
void initGames();
g_extr Game* goalie;
g_extr Game* striker;
g_extr Game* keeper;

12
include/strategy_roles/goalie.h → include/strategy_roles/striker.h
View File

@ -5,9 +5,9 @@
#include "strategy_roles/game.h"
#define TILT_MULT 1.8
#define TILT_DIST 180
#define TILT_DIST 170
#define CATCH_DIST 150
#define GOALIE_ATKSPD_LAT 255
#define GOALIE_ATKSPD_LAT 320 //255
#define GOALIE_ATKSPD_BAK 350
#define GOALIE_ATKSPD_FRT 345
#define GOALIE_ATKSPD_STRK 355
@ -19,16 +19,16 @@
#define GOALIE_ATKDIR_PLUSANG2_COR 70
#define GOALIE_ATKDIR_PLUSANG3_COR 70
class Goalie : public Game, public PositionSysZone{
class Striker : public Game, public PositionSysZone{
public:
Goalie();
Goalie(LineSystem* ls, PositionSystem* ps);
Striker();
Striker(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void goalie(int);
void striker();
void ballBack();
void storcimentoPorta();

2
include/sensors/linesys_2019.h → include/systems/lines/linesys_2019.h
View File

@ -3,7 +3,7 @@
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "position/systems.h"
#include "systems/systems.h"
#include "vars.h"

4
include/sensors/linesys_camera.h → include/systems/lines/linesys_camera.h
View File

@ -3,7 +3,7 @@
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "position/systems.h"
#include "systems/systems.h"
#include "vars.h"
@ -17,7 +17,7 @@
#define S4O 22 //A2
#define LINE_THRESH_CAM 90
#define EXIT_TIME 100
#define EXIT_TIME 125
#define LINES_EXIT_SPD 350
class LineSysCamera : public LineSystem{

40
include/systems/lines/linesys_camera_new.h Normal file
View File

@ -0,0 +1,40 @@
// #pragma once
// #include <Arduino.h>
// #include "behaviour_control/ds_ctrl.h"
// #include "position/systems.h"
// #include "vars.h"
// #define S1I A14
// #define S1O A15
// #define S2I A16
// #define S2O A17
// #define S3I A20
// #define S3O A0
// #define S4I A1
// #define S4O A2
// #define LINE_THRESH_CAM 90
// #define EXTIME_CAM 75
// #define LINES_EXIT_SPD_CAM 300
// class LineSysCamera : public LineSystem{
// public:
// LineSysCamera();
// LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out);
// void update() override;
// void test() override;
// void outOfBounds();
// private:
// vector<DataSource*> in, out;
// DataSource* ds;
// bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
// int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i;
// unsigned long exitTimer;
// byte linesens;
// };

20
include/position/positionsys_camera.h → include/systems/position/positionsys_camera.h
View File

@ -1,11 +1,17 @@
#include "PID_v2.h"
#include "position/systems.h"
#include "systems/systems.h"
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0
//Note: those variables can be changes, and will need to change depending on camera calibration
//Camera center: those setpoints correspond to the center of the field
#define CAMERA_CENTER_X -5
#define CAMERA_CENTER_Y -17
//Camera goal: those setpoints correspond to the position of the center of the goal on the field
#define CAMERA_GOAL_X 0
#define CAMERA_GOAL_Y -13
#define CAMERA_GOAL_Y -20
#define CAMERA_CENTER_Y_ABS_SUM 72
//Actually it's ± MAX_VAL
#define MAX_X 25
@ -24,6 +30,9 @@ class PositionSysCamera : public PositionSystem{
public:
PositionSysCamera();
void goCenter();
void centerGoal();
void setMoveSetpoints(int x, int y);
void addMoveOnAxis(int x, int y);
void update() override;
void test() override;
void setCameraPID();
@ -31,7 +40,8 @@ class PositionSysCamera : public PositionSystem{
int calcOtherGoalY(int goalY);
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
int MAX_DIST;
int MAX_DIST, vx, vy, axisx, axisy;
bool givenMovement;
PID* X;
PID* Y;

2
include/position/positionsys_zone.h → include/systems/position/positionsys_zone.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "position/systems.h"
#include "systems/systems.h"
//POSITION
#define CENTERGOALPOST_VEL1 220

0
include/position/systems.h → include/systems/systems.h
View File

8
include/vars.h
View File

@ -1,6 +1,12 @@
#pragma once
#define DEBUG Serial
#ifdef VARS
#define extr
#else
#define extr extern
#endif
#define GLOBAL_SPD_MULT 1.0
/*SWS and LEDS are to be tested and implemented in the code.
@ -19,3 +25,5 @@ in the 32U4 code*/
#define SWITCH_1 39
#define SWITCH_2 38
#define SWITCH_3 33*/
extr float sins[360], cosins[360];

26
src/main.cpp
View File

@ -1,15 +1,25 @@
#include <Arduino.h>
#include "test_menu.h"
#define VARS
#include "behaviour_control/status_vector.h"
#include "position/positionsys_zone.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
#include "vars.h"
#include "test_menu.h"
TestMenu* testmenu;
void setup() {
delay(1500);
DEBUG.begin(9600);
for(int i = 0; i < 360; i++){
sins[i] = (float) sin((i*3.14/180));
cosins[i] = (float) cos((i*3.14/180));
}
testmenu = new TestMenu();
initStatusVector();
initSensors();
initGames();
@ -20,14 +30,12 @@ void setup() {
void loop() {
updateSensors();
/* TestMenu testmenu;
testmenu.testMenu();
goalie->play(role==1);
keeper->play(role==0);
if(DEBUG.available()) testmenu->testMenu();
camera->test();
striker->play(role==1);
keeper->play(role==0);
// Last thing to do: movement and update status vector
drive->drivePrepared();
updateStatusVector(); */
updateStatusVector();
}

13
src/motors_movement/drivecontroller.cpp
View File

@ -1,6 +1,7 @@
#include "motors_movement/drivecontroller.h"
#include "sensors/sensors.h"
#include "behaviour_control/status_vector.h"
#include "vars.h"
DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
m1 = m1_;
@ -8,11 +9,6 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
m3 = m3_;
m4 = m4_;
for(int i = 0; i < 360; i++){
sins[i] = (float) sin(torad(i));
cosins[i] = (float) cos(torad(i));
}
pDir = 0;
pSpeed = 0;
pTilt = 0;
@ -69,6 +65,11 @@ void DriveController::drive(int dir, int speed, int tilt){
speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT;
tilt = tilt > 180 ? tilt - 360 : tilt;
//TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
// Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
// Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
// vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
// vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
@ -87,7 +88,7 @@ void DriveController::drive(int dir, int speed, int tilt){
speed3 = -(speed1);
speed4 = -(speed2);
// calcola l'errore di posizione rispetto allo 0
// Calculate position error relative to the 0
delta = CURRENT_DATA_READ.IMUAngle;
if(delta > 180) delta = delta - 360;

120
src/position/positionsys_camera.cpp
View File

@ -1,120 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "vars.h"
PositionSysCamera::PositionSysCamera() {
setCameraPID();
}
void PositionSysCamera::update(){
}
void PositionSysCamera::test(){
}
void PositionSysCamera::goCenter(){
/*WORKS BUT CAN BE BETTER*/
//Y
/* 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);
//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(270, 75, 0);
else drive->prepareDrive(0, 0, 0); */
/*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)
y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
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);
dir = (dir+360) % 360;
drive->prepareDrive(dir, 100, 0);
} */
CameraPID();
}
//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;
Inputy = 0;
Outputy = 0;
Setpointy = 0;
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
X->SetOutputLimits(-50,50);
X->SetMode(AUTOMATIC);
X->SetDerivativeLag(1);
X->SetSampleTime(2);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE);
Y->SetOutputLimits(-50,50);
Y->SetMode(AUTOMATIC);
Y->SetDerivativeLag(1);
Y->SetSampleTime(2);
}
/*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
The sign of the goal y we found is simply the reverse of the one we got
*/
int PositionSysCamera::calcOtherGoalY(int goalY){
int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY);
otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY;
return otherGoalY;
}
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;
}else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
Inputx = CURRENT_DATA_READ.cam_xb;
Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb);
}else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputx = CURRENT_DATA_READ.cam_xy;
Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
//Setpointy todo
}else{
//TODO: no goal seen
}
//To have the axis corresponding to the real cartesian plane Inputx and InputY must be multiplied by -1
//This is because the coordinates given as input are relative to the robot, while the coordinates used in the setpoint
//are on an absolute cartesian plane with it's center in the center of the field
Inputx *= -1;
Inputy *= -1;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y;
X->Compute();
Y->Compute();
// DEBUG.print(CURRENT_DATA_READ.cam_yb);
// DEBUG.print(" ");
// DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
/*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
drive->prepareDrive(0,0,0);
}else{*/
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
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);
//}
}

2
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -45,6 +45,8 @@ void DataSourceCameraConic ::readSensor() {
true_yb = yb;
true_xy = xy;
true_yy = yy;
computeCoordsAngles();
}
end = true;
start = false;

5
src/strategy_roles/game.cpp
View File

@ -1,5 +1,6 @@
#include "behaviour_control/status_vector.h"
#include "strategy_roles/game.h"
#include "systems/position/positionsys_camera.h"
Game::Game() {}
Game::Game(LineSystem* ls_, PositionSystem* ps_) {
@ -8,14 +9,14 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) {
}
void Game::play(bool condition){
ps->update();
if(condition) {
ps->update();
realPlay();
ls->update();
CURRENT_DATA_WRITE.posSystem = (this->ps);
CURRENT_DATA_WRITE.lineSystem = (this->ls);
CURRENT_DATA_WRITE.game = this;
((PositionSysCamera*)ps)->CameraPID();
}
}

8
src/strategy_roles/games.cpp
View File

@ -1,15 +1,15 @@
#define GAMES_CPP
/* #include "sensors/linesys_2019.h" */
#include "sensors/linesys_camera.h"
#include "position/positionsys_zone.h"
#include "position/positionsys_camera.h"
#include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_zone.h"
#include "systems/position/positionsys_camera.h"
#include "strategy_roles/games.h"
void initGames(){
vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
goalie = new Goalie(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

61
src/strategy_roles/goalie.cpp
View File

@ -1,61 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/goalie.h"
#include "vars.h"
#include "math.h"
Goalie::Goalie() : Game() {
init();
}
Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
init();
}
void Goalie::init(){
atk_speed = 0;
atk_direction = 0;
cstorc = 0;
}
void Goalie::realPlay(){
if(ball->ballSeen) this->goalie(50);
else ((PositionSysCamera*)ps)->goCenter();
}
int dir, degrees2;
void Goalie::goalie(int plusang) {
if(ball->distance < CATCH_DIST) drive->prepareDrive(ball->angle, 350, 0);
else{
/* if(ball->angle > 345 || ball->angle < 15) plusang *= 0.15;
FRONT */
if (ball->angle > 345 && ball->angle < 15) plusang *= 0.15;
if(ball->angle > 180) degrees2 = ball->angle - 360;
else degrees2 = ball->angle;
if(degrees2 > 0) dir = ball->angle + plusang; //45 con 8 ruote
else dir = ball->angle - plusang; //45 con 8 ruote
if(dir < 0) dir = dir + 360;
else dir = dir;
storcimentoPorta();
if(ball->distance > TILT_DIST && (ball->angle > 340 || ball->angle < 20)){
plusang -= 20;
drive->prepareDrive(dir, 350, cstorc);
} else {
drive->prepareDrive(dir, 350, 0);
cstorc = 0;
}
}
}
void Goalie::storcimentoPorta() {
if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9;
else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9;
else cstorc *= 0.9;
cstorc = constrain(cstorc, -45, 45);
}

7
src/strategy_roles/keeper.cpp
View File

@ -1,10 +1,11 @@
#include <Arduino.h>
#include "behaviour_control/status_vector.h"
#include "sensors/linesys_2019.h"
#include "systems/lines/linesys_2019.h"
#include "sensors/sensors.h"
#include "strategy_roles/keeper.h"
#include "strategy_roles/games.h"
#include "systems/position/positionsys_camera.h"
Keeper::Keeper() : Game() {
@ -28,14 +29,14 @@ void Keeper::init(){
void Keeper::realPlay() {
if(ball->ballSeen) keeper();
else drive->prepareDrive(0,0,0);
else ((PositionSysCamera*)ps)->centerGoal();
}
void Keeper::keeper() {
if(ball->distance > KEEPER_ATTACK_DISTANCE){
// Ball is quite near
goalie->play();
striker->play();
if(!this->ls->tookLine){
keeperAttackTimer = 0;
keeper_tookTimer = true;

99
src/strategy_roles/striker.cpp Normal file
View File

@ -0,0 +1,99 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/striker.h"
#include "vars.h"
#include "math.h"
Striker::Striker() : Game() {
init();
}
Striker::Striker(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
init();
}
void Striker::init(){
atk_speed = 0;
atk_direction = 0;
cstorc = 0;
}
void Striker::realPlay(){
if(CURRENT_DATA_READ.ballSeen) this->striker();
else ((PositionSysCamera*)ps)->goCenter();
}
void Striker::striker() {
if(CURRENT_DATA_READ.ballAngle>= 350 || CURRENT_DATA_READ.ballAngle<= 20) {
if(CURRENT_DATA_READ.ballDistance > 190) atk_direction = 0;
else atk_direction = CURRENT_DATA_READ.ballAngle;
atk_speed = GOALIE_ATKSPD_FRT;
}
if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) {
ballBack();
atk_speed = GOALIE_ATKSPD_BAK;
}
if(CURRENT_DATA_READ.ballAngle> 10 && CURRENT_DATA_READ.ballAngle< 30) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle>= 30 && CURRENT_DATA_READ.ballAngle< 45) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle>= 45 && CURRENT_DATA_READ.ballAngle< 90) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 270 && CURRENT_DATA_READ.ballAngle<= 315) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 315 && CURRENT_DATA_READ.ballAngle<= 330) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 330 && CURRENT_DATA_READ.ballAngle< 350) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
// if((CURRENT_DATA_READ.ballAngle>= 330 || CURRENT_DATA_READ.ballAngle<= 30) && CURRENT_DATA_READ.ballDistance > 190) { //storcimento
// atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito
// drive->prepareDrive(atk_direction, atk_speed, cstorc);
// }
// else
drive->prepareDrive(atk_direction, atk_speed);
}
void Striker::ballBack() {
int ball_degrees2;
int dir;
int plusang;
if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK;
else plusang = 0;
if(CURRENT_DATA_READ.ballAngle> 180) ball_degrees2 = CURRENT_DATA_READ.ballAngle- 360;
else ball_degrees2 = CURRENT_DATA_READ.ballAngle;
if(ball_degrees2 > 0) dir = CURRENT_DATA_READ.ballAngle+ plusang; //45 con 8 ruote
else dir = CURRENT_DATA_READ.ballAngle- plusang; //45 con 8 ruote
if(dir < 0) dir = dir + 360;
else dir = dir;
atk_direction = dir;
}
void Striker::storcimentoPorta() {
if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9;
else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9;
else cstorc *= 0.9;
cstorc = constrain(cstorc, -45, 45);
}

2
src/sensors/linesys_2019.cpp → src/system/lines/linesys_2019.cpp
View File

@ -1,4 +1,4 @@
#include "sensors/linesys_2019.h"
#include "systems/lines/linesys_2019.h"
#include "sensors/sensors.h"
using namespace std;

6
src/sensors/linesys_camera.cpp → src/system/lines/linesys_camera.cpp
View File

@ -1,5 +1,5 @@
#include "sensors/linesys_camera.h"
#include "position/positionsys_camera.h"
#include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
@ -78,7 +78,7 @@ void LineSysCamera::outOfBounds(){
}
if (exitTimer <= EXTIME){
((PositionSysCamera*)goalie->ps)->goCenter();
((PositionSysCamera*)striker->ps)->goCenter();
tookLine = true;
}else{
drive->canUnlock = true;

116
src/system/lines/linesys_camera_new.cpp Normal file
View File

@ -0,0 +1,116 @@
// #include "sensors/linesys_camera.h"
// #include "position/positionsys_camera.h"
// #include "sensors/sensors.h"
// #include "strategy_roles/games.h"
// using namespace std;
// LineSysCamera::LineSysCamera() {}
// LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){
// this->in = in_;
// this->out = out_;
// fboundsX = false;
// fboundsY = false;
// slow = false;
// linesensOldX = 0;
// linesensOldY = 0;
// tookLine = false;
// for(int i = 0; i < 4; i++){
// linetriggerI[i] = 0;
// linetriggerO[i] = 0;
// }
// exitTimer = 0;
// linesens = 0;
// }
// void LineSysCamera::update(){
// inV = 0;
// outV = 0;
// tookLine = false;
// for(DataSource* d : in) d->readSensor();
// for(DataSource* d : out) d->readSensor();
// for(auto it = in.begin(); it != in.end(); it++){
// i = it - in.begin();
// ds = *it;
// linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
// }
// for(auto it = out.begin(); it != out.end(); it++){
// i = it - out.begin();
// ds = *it;
// linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
// }
// for(int i = 0; i < 4; i++){
// inV = inV | (linetriggerI[i] << i);
// outV = outV | (linetriggerO[i] << i);
// }
// if ((inV > 0) || (outV > 0)) {
// if(millis() - exitTimer > EXTIME_CAM) {
// fboundsX = true;
// fboundsY = true;
// }
// exitTimer = millis();
// }
// linesens |= inV | outV;
// outOfBounds();
// }
// void LineSysCamera::outOfBounds(){
// if(fboundsX == true) {
// if(linesens & 0x02) linesensOldX = 2;
// else if(linesens & 0x08) linesensOldX = 8;
// if(linesensOldX != 0) fboundsX = false;
// }
// if(fboundsY == true) {
// if(linesens & 0x01) linesensOldY = 1;
// else if(linesens & 0x04) linesensOldY = 4;
// if(linesensOldY != 0) fboundsY = false;
// }
// if (millis() - exitTimer <= EXTIME_CAM){
// if(linesens > 0) ((PositionSysCamera*)goalie->ps)->goCenter();
// tookLine = true;
// }else{
// linesens = 0;
// linesensOldY = 0;
// linesensOldX = 0;
// }
// }
// void LineSysCamera::test(){
// update();
// DEBUG.print("In: ");
// for(DataSource* d : in){
// d->update();
// DEBUG.print(d->getValue());
// DEBUG.print(" | ");
// }
// DEBUG.print(" |---| ");
// DEBUG.print("Out: ");
// for(DataSource* d : out){
// d->update();
// DEBUG.print(d->getValue());
// DEBUG.print(" | ");
// }
// DEBUG.println();
// for(int i = 0; i < 4; i++){
// DEBUG.print(linetriggerI[i]);
// DEBUG.print(" | ");
// DEBUG.println(linetriggerO[i]);
// }
// DEBUG.println(inV);
// DEBUG.println(outV);
// DEBUG.println();
// }

135
src/system/positions/positionsys_camera.cpp Normal file
View File

@ -0,0 +1,135 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "vars.h"
PositionSysCamera::PositionSysCamera() {
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
Inputx = 0;
Outputx = 0;
Setpointx = 0;
Inputy = 0;
Outputy = 0;
Setpointy = 0;
vx = 0;
vy = 0;
axisx = 0;
axisy = 0;
givenMovement = false;
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
X->SetOutputLimits(-50,50);
X->SetMode(AUTOMATIC);
X->SetDerivativeLag(1);
X->SetSampleTime(2);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE);
Y->SetOutputLimits(-50,50);
Y->SetMode(AUTOMATIC);
Y->SetDerivativeLag(1);
Y->SetSampleTime(2);
}
void PositionSysCamera::update(){
int posx, posy;
//Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync
//Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot
if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){
posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2;
posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy;
}else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){
posx = CURRENT_DATA_WRITE.cam_xb;
posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb);
}else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){
posx = CURRENT_DATA_WRITE.cam_xy;
posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
}else{
//TODO: no goal seen ?
}
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
CURRENT_DATA_WRITE.posx = posx;
CURRENT_DATA_WRITE.posy = posy;
Inputx = posx;
Inputy = posy;
//Prepare for receiving information about movement
//Starting setpoint position as current position
Setpointx = posx;
Setpointy = posy;
axisx = 0;
axisy = 0;
givenMovement = false;
}
//This means the last time this is called has the biggest priority, has for prepareDrive
void PositionSysCamera::setMoveSetpoints(int x, int y){
Setpointx = x;
Setpointy = y;
givenMovement = true;
}
void PositionSysCamera::addMoveOnAxis(int x, int y){
axisx += x;
axisy += y;
givenMovement = true;
}
void PositionSysCamera::goCenter(){
setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
}
void PositionSysCamera::centerGoal(){
setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y);
}
/*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
The sign of the goal y we found is simply the reverse of the one we got
*/
int PositionSysCamera::calcOtherGoalY(int goalY){
int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY);
otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY;
return otherGoalY;
}
void PositionSysCamera::CameraPID(){
if(givenMovement){
vx = 0;
vy = 0;
Setpointx += axisx;
Setpointy += axisy;
X->Compute();
Y->Compute();
//Compute an X and Y to give to the PID later
//There's surely a better way to do this
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
drive->prepareDrive(dir, speed, 0);
//Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
//and check the notes in drivecontroller for the other stuff to comment and uncomment
//TODO: add complementary filter on this speed if we keep using it
// vx = ((speed * cosins[dir]));
// vy = ((-speed * sins[dir]));
// CURRENT_DATA_WRITE.addvx = vx;
// CURRENT_DATA_WRITE.addvy = vy;
}
}
void PositionSysCamera::test(){
}

2
src/position/positionsys_zone.cpp → src/system/positions/positionsys_zone.cpp
View File

@ -1,5 +1,5 @@
#include "behaviour_control/status_vector.h"
#include "position/positionsys_zone.h"
#include "systems/position/positionsys_zone.h"
#include "sensors/sensors.h"
#include "vars.h"

8
src/test_menu.cpp
View File

@ -3,12 +3,12 @@
#include "sensors/data_source_bno055.h"
#include "sensors/data_source_bt.h"
#include "sensors/data_source_camera_conicmirror.h"
#include "sensors/linesys_camera.h"
#include "sensors/linesys_2019.h"
#include "systems/lines/linesys_camera.h"
#include "systems/lines/linesys_2019.h"
#include "sensors/sensors.h"
#include "motors_movement/motor.h"
#include "motors_movement/drivecontroller.h"
#include "position/positionsys_camera.h"
#include "systems/position/positionsys_camera.h"
#include "strategy_roles/game.h"
#include "strategy_roles/games.h"
#include "behaviour_control/data_source.h"
@ -91,7 +91,7 @@ void TestMenu :: testMenu(){
currentRole = DEBUG.read();
switch(currentRole){
case '1':
(goalie->ls)->test();
(striker->ls)->test();
break;
case '2':
(keeper->ls)->test();

12
utility/OpenMV/conic_eff.py
View File

@ -38,8 +38,8 @@ blue_led.on()
##############################################################################
thresholds = [ (50, 98, -2, 30, 57, 113), # thresholds yellow goal
(22, 45, -32, -6, -16, 10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal
(38, 55, -33, -1, 0, 26)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
@ -59,12 +59,12 @@ clock = time.clock()'''
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(1)
sensor.set_saturation(1)
sensor.set_contrast(3)
sensor.set_saturation(3)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 6500)
sensor.set_auto_exposure(False, 5500)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)
@ -84,7 +84,7 @@ while(True):
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=40, area_threshold=50, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=40, merge = True):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())

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 = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal
(38, 55, -33, -1, 0, 26)] # 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(3)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 5500)
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=50, 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)