From f62d013675342614515471c98717bfae820598c1 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Thu, 12 Mar 2020 12:09:13 +0100 Subject: [PATCH 1/7] position system camera and vector sum system to do multiple movements in a single one. obv to be tested, but it's difficult without robot and field --- include/behaviour_control/status_vector.h | 3 +- include/motors_movement/drivecontroller.h | 2 - include/position/positionsys_camera.h | 6 +- include/vars.h | 11 +- src/main.cpp | 10 +- src/motors_movement/drivecontroller.cpp | 11 +- src/position/positionsys_camera.cpp | 162 ++++++++++-------- .../data_source_camera_conicmirror.cpp | 2 + src/strategy_roles/game.cpp | 5 +- 9 files changed, 122 insertions(+), 90 deletions(-) diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index f20048d..3a67b28 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -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; diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index a951194..280d851 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -51,6 +51,4 @@ class DriveController{ double input, output, setpoint; - float sins[360], cosins[360]; - }; \ No newline at end of file diff --git a/include/position/positionsys_camera.h b/include/position/positionsys_camera.h index ad3073b..d75673f 100644 --- a/include/position/positionsys_camera.h +++ b/include/position/positionsys_camera.h @@ -24,6 +24,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 +34,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; diff --git a/include/vars.h b/include/vars.h index 4c4e00f..4c42379 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,6 +1,12 @@ #pragma once #define DEBUG Serial3 +#ifdef VARS +#define extr +#else +#define extr extern +#endif + #define GLOBAL_SPD_MULT 1.0 #define LED_R 20 @@ -9,4 +15,7 @@ #define BUZZER 30 #define SWITCH_SX 28 -#define SWITCH_DX 29 \ No newline at end of file +#define SWITCH_DX 29 + + +extr float sins[360], cosins[360]; diff --git a/src/main.cpp b/src/main.cpp index 1045fff..d44002f 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,14 +1,22 @@ #include +#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" 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)); + } + initStatusVector(); initSensors(); initGames(); diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 98773b5..23991a0 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -1,17 +1,13 @@ #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_; m2 = m2_; 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; @@ -69,8 +65,9 @@ void DriveController::drive(int dir, int speed, int tilt){ speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT; tilt = tilt > 180 ? tilt - 360 : tilt; - vx = ((speed * cosins[dir])); - vy = ((-speed * sins[dir])); + //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE? + vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; + vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { vxn = 0; diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp index ba28ab8..7fd606b 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/position/positionsys_camera.cpp @@ -4,46 +4,6 @@ #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; @@ -51,6 +11,12 @@ void PositionSysCamera :: setCameraPID(){ 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); @@ -64,9 +30,65 @@ void PositionSysCamera :: setCameraPID(){ 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 +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){ @@ -75,46 +97,36 @@ int PositionSysCamera::calcOtherGoalY(int goalY){ 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 - } +void PositionSysCamera::CameraPID(){ + if(givenMovement){ - //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; + vx = 0; + vy = 0; - Setpointx = CAMERA_CENTER_X; - Setpointy = CAMERA_CENTER_Y; + Setpointx += axisx; + Setpointy += axisy; + + X->Compute(); + Y->Compute(); - 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{*/ + //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); - //DEBUG.println(dir); - //} -} \ No newline at end of file + + //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(){ +} diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index d4b7b4e..36f4e18 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -45,6 +45,8 @@ void DataSourceCameraConic ::readSensor() { true_yb = yb; true_xy = xy; true_yy = yy; + + computeCoordsAngles(); } end = true; start = false; diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp index 397c8b3..f9b5b05 100644 --- a/src/strategy_roles/game.cpp +++ b/src/strategy_roles/game.cpp @@ -1,5 +1,6 @@ #include "behaviour_control/status_vector.h" #include "strategy_roles/game.h" +#include "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(); } } \ No newline at end of file From 0f913d8ad12f3a377d728978ccfda5a50b91dbe4 Mon Sep 17 00:00:00 2001 From: "ema.coletta@gmail.com" Date: Thu, 29 Oct 2020 17:19:20 +0100 Subject: [PATCH 2/7] implement test menu in main loop --- src/main.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 4ff75dc..cd37a1e 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,6 +7,9 @@ #include "sensors/sensors.h" #include "strategy_roles/games.h" #include "vars.h" +#include "test_menu.h" + +TestMenu* testmenu; void setup() { delay(1500); @@ -17,6 +20,7 @@ void setup() { cosins[i] = (float) cos((i*3.14/180)); } + testmenu = new TestMenu(); initStatusVector(); initSensors(); initGames(); @@ -27,8 +31,8 @@ void setup() { void loop() { updateSensors(); - //TestMenu testmenu; - //testmenu.testMenu(); + if(DEBUG.available()) testmenu->testMenu(); + goalie->play(role==1); keeper->play(role==0); From 304610e2e22b6aae70fa98f6021d50aa4fac9327 Mon Sep 17 00:00:00 2001 From: "ema.coletta@gmail.com" Date: Thu, 29 Oct 2020 17:20:36 +0100 Subject: [PATCH 3/7] recycle striker strategy from last year code bad translation, goalie is actually a striker --- include/strategy_roles/goalie.h | 4 +- src/strategy_roles/goalie.cpp | 88 +++++++++++++++++++++++---------- 2 files changed, 65 insertions(+), 27 deletions(-) diff --git a/include/strategy_roles/goalie.h b/include/strategy_roles/goalie.h index e75f3fd..aee10d5 100644 --- a/include/strategy_roles/goalie.h +++ b/include/strategy_roles/goalie.h @@ -7,7 +7,7 @@ #define TILT_MULT 1.8 #define TILT_DIST 180 #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 @@ -28,7 +28,7 @@ class Goalie : public Game, public PositionSysZone{ private: void realPlay() override; void init() override; - void goalie(int); + void goalie(); void ballBack(); void storcimentoPorta(); diff --git a/src/strategy_roles/goalie.cpp b/src/strategy_roles/goalie.cpp index cbcc3d7..a62c24f 100644 --- a/src/strategy_roles/goalie.cpp +++ b/src/strategy_roles/goalie.cpp @@ -22,37 +22,75 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(50); + if(ball->ballSeen) this->goalie(); 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::goalie() { + if(ball->angle>= 350 || ball->angle<= 20) { + if(ball->distance > 190) atk_direction = 0; + else atk_direction = ball->angle; + atk_speed = GOALIE_ATKSPD_FRT; } + + if(ball->angle>= 90 && ball->angle<= 270) { + ballBack(); + atk_speed = GOALIE_ATKSPD_BAK; + } + + if(ball->angle> 10 && ball->angle< 30) { + atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG1; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle>= 30 && ball->angle< 45) { + atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG2; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle>= 45 && ball->angle< 90) { + atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG3; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle> 270 && ball->angle<= 315) { + atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG3_COR; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle> 315 && ball->angle<= 330) { + atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG2_COR; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle> 330 && ball->angle< 350) { + atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG1_COR; + atk_speed = GOALIE_ATKSPD_LAT; + } + + // if((ball->angle>= 330 || ball->angle<= 30) && ball->distance > 190) { //storcimento + // atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito + // drive->prepareDrive(atk_direction, atk_speed, cstorc); + // } + // else + drive->prepareDrive(atk_direction, 100); } + + +void Goalie::ballBack() { + int ball_degrees2; + int dir; + + int plusang; + if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; + else plusang = 0; + + if(ball->angle> 180) ball_degrees2 = ball->angle- 360; + else ball_degrees2 = ball->angle; + if(ball_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; + atk_direction = dir; +} + + 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; From bc040bd084e1ae0ea6aa333303c1c198873a6df0 Mon Sep 17 00:00:00 2001 From: "ema.coletta@gmail.com" Date: Thu, 29 Oct 2020 22:39:39 +0100 Subject: [PATCH 4/7] goalie: use status vector's CURRENT_DATA_READ to get sensors data So this is in sync with the rest of the data being used in the cycle. This runs a loop cycle behind reality, but we don't care on such a fast machine Also use atk_speed instead of hardcoding it to 100 --- src/strategy_roles/goalie.cpp | 48 +++++++++++++++++------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/strategy_roles/goalie.cpp b/src/strategy_roles/goalie.cpp index a62c24f..9ef0f3e 100644 --- a/src/strategy_roles/goalie.cpp +++ b/src/strategy_roles/goalie.cpp @@ -22,53 +22,53 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(); + if(CURRENT_DATA_READ.ballSeen) this->goalie(); else ((PositionSysCamera*)ps)->goCenter(); } void Goalie::goalie() { - if(ball->angle>= 350 || ball->angle<= 20) { - if(ball->distance > 190) atk_direction = 0; - else atk_direction = ball->angle; + 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(ball->angle>= 90 && ball->angle<= 270) { + if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) { ballBack(); atk_speed = GOALIE_ATKSPD_BAK; } - if(ball->angle> 10 && ball->angle< 30) { - atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG1; + 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(ball->angle>= 30 && ball->angle< 45) { - atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG2; + 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(ball->angle>= 45 && ball->angle< 90) { - atk_direction = ball->angle+ GOALIE_ATKDIR_PLUSANG3; + 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(ball->angle> 270 && ball->angle<= 315) { - atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG3_COR; + 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(ball->angle> 315 && ball->angle<= 330) { - atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG2_COR; + 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(ball->angle> 330 && ball->angle< 350) { - atk_direction = ball->angle- GOALIE_ATKDIR_PLUSANG1_COR; + 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((ball->angle>= 330 || ball->angle<= 30) && ball->distance > 190) { //storcimento + // 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, 100); + drive->prepareDrive(atk_direction, atk_speed); } @@ -78,13 +78,13 @@ void Goalie::ballBack() { int dir; int plusang; - if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; + if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; else plusang = 0; - if(ball->angle> 180) ball_degrees2 = ball->angle- 360; - else ball_degrees2 = ball->angle; - if(ball_degrees2 > 0) dir = ball->angle+ plusang; //45 con 8 ruote - else dir = ball->angle- plusang; //45 con 8 ruote + 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; From f0a8ad6808a446146e1378ac99c802c2a75295d7 Mon Sep 17 00:00:00 2001 From: "ema.coletta@gmail.com" Date: Sat, 31 Oct 2020 15:37:05 +0100 Subject: [PATCH 5/7] disable vector sum code for now calculations are a bitty crappy, think I found the bug for now (check added comments) --- src/motors_movement/drivecontroller.cpp | 10 +++++++--- src/position/positionsys_camera.cpp | 13 ++++++++----- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 23991a0..45df50e 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -66,8 +66,12 @@ void DriveController::drive(int dir, int speed, int tilt){ tilt = tilt > 180 ? tilt - 360 : tilt; //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE? - vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; - vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; + // 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])); if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { vxn = 0; @@ -84,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; diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp index 7fd606b..f6ee95c 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/position/positionsys_camera.cpp @@ -118,13 +118,16 @@ void PositionSysCamera::CameraPID(){ 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])); - vx = ((speed * cosins[dir])); - vy = ((-speed * sins[dir])); - - CURRENT_DATA_WRITE.addvx = vx; - CURRENT_DATA_WRITE.addvy = vy; + // CURRENT_DATA_WRITE.addvx = vx; + // CURRENT_DATA_WRITE.addvy = vy; } } From c0b8771c470bd8dea2307e1613708639d368a532 Mon Sep 17 00:00:00 2001 From: "ema.coletta@gmail.com" Date: Sat, 31 Oct 2020 15:37:38 +0100 Subject: [PATCH 6/7] a bit of recalibration for everything for the presentation game --- include/position/positionsys_camera.h | 12 ++- include/sensors/linesys_camera.h | 2 +- include/sensors/linesys_camera_new.h | 40 +++++++++ include/strategy_roles/goalie.h | 2 +- src/sensors/linesys_camera_new.cpp | 116 ++++++++++++++++++++++++++ src/strategy_roles/keeper.cpp | 3 +- utility/OpenMV/conic_eff.py | 14 ++-- 7 files changed, 176 insertions(+), 13 deletions(-) create mode 100644 include/sensors/linesys_camera_new.h create mode 100644 src/sensors/linesys_camera_new.cpp diff --git a/include/position/positionsys_camera.h b/include/position/positionsys_camera.h index d75673f..a5bb905 100644 --- a/include/position/positionsys_camera.h +++ b/include/position/positionsys_camera.h @@ -2,10 +2,16 @@ #include "position/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 1 +#define CAMERA_CENTER_Y -10 + +//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 diff --git a/include/sensors/linesys_camera.h b/include/sensors/linesys_camera.h index 6a301f3..333ea1d 100644 --- a/include/sensors/linesys_camera.h +++ b/include/sensors/linesys_camera.h @@ -17,7 +17,7 @@ #define S4O A2 #define LINE_THRESH_CAM 90 -#define EXIT_TIME 100 +#define EXIT_TIME 125 #define LINES_EXIT_SPD 350 class LineSysCamera : public LineSystem{ diff --git a/include/sensors/linesys_camera_new.h b/include/sensors/linesys_camera_new.h new file mode 100644 index 0000000..8de96bd --- /dev/null +++ b/include/sensors/linesys_camera_new.h @@ -0,0 +1,40 @@ +// #pragma once + +// #include + +// #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 in_, vector out); + +// void update() override; +// void test() override; +// void outOfBounds(); + +// private: +// vector 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; +// }; \ No newline at end of file diff --git a/include/strategy_roles/goalie.h b/include/strategy_roles/goalie.h index aee10d5..cefb904 100644 --- a/include/strategy_roles/goalie.h +++ b/include/strategy_roles/goalie.h @@ -5,7 +5,7 @@ #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 320 //255 #define GOALIE_ATKSPD_BAK 350 diff --git a/src/sensors/linesys_camera_new.cpp b/src/sensors/linesys_camera_new.cpp new file mode 100644 index 0000000..5a906fe --- /dev/null +++ b/src/sensors/linesys_camera_new.cpp @@ -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 in_, vector 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(); +// } \ No newline at end of file diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index 6d0040b..7e77f28 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -5,6 +5,7 @@ #include "sensors/sensors.h" #include "strategy_roles/keeper.h" #include "strategy_roles/games.h" +#include "position/positionsys_camera.h" Keeper::Keeper() : Game() { @@ -28,7 +29,7 @@ void Keeper::init(){ void Keeper::realPlay() { if(ball->ballSeen) keeper(); - else drive->prepareDrive(0,0,0); + else ((PositionSysCamera*)ps)->centerGoal(); } void Keeper::keeper() { diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 0fdfd21..b04961c 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -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 = [ (58, 97, 9, 45, 51, 92), # thresholds yellow goal + (12, 30, -25, 1, -49, -2)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -59,13 +59,13 @@ 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_gain(True) +sensor.set_auto_exposure(False, 4500) +sensor.set_auto_gain(False, 15) sensor.skip_frames(time = 300) clock = time.clock() @@ -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()) From 23540152013c03fd995f085892cdda306204dd9d Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 4 Nov 2020 17:46:14 +0100 Subject: [PATCH 7/7] reorder files into system folders also rename Goalie to Striker because GRAMMAR (Goalie actually means keeper but we always intended it as a striker :D) --- include/behaviour_control/status_vector.h | 2 +- include/sensors/sensors.h | 8 +- include/strategy_roles/game.h | 2 +- include/strategy_roles/games.h | 4 +- .../strategy_roles/{goalie.h => striker.h} | 8 +- .../{sensors => systems/lines}/linesys_2019.h | 2 +- .../lines}/linesys_camera.h | 2 +- .../lines}/linesys_camera_new.h | 0 .../position/positionsys_camera.h | 6 +- .../{ => systems}/position/positionsys_zone.h | 2 +- include/{position => systems}/systems.h | 0 src/main.cpp | 3 +- src/strategy_roles/game.cpp | 2 +- src/strategy_roles/games.cpp | 8 +- src/strategy_roles/keeper.cpp | 6 +- .../{goalie.cpp => striker.cpp} | 20 +-- .../lines}/linesys_2019.cpp | 2 +- .../lines}/linesys_camera.cpp | 6 +- .../lines}/linesys_camera_new.cpp | 0 .../positions}/positionsys_camera.cpp | 2 +- .../positions}/positionsys_zone.cpp | 2 +- src/test_menu.cpp | 8 +- utility/OpenMV/conic_eff.py | 8 +- utility/OpenMV/conic_eff.py.autosave | 149 ++++++++++++++++++ 24 files changed, 200 insertions(+), 52 deletions(-) rename include/strategy_roles/{goalie.h => striker.h} (84%) rename include/{sensors => systems/lines}/linesys_2019.h (96%) rename include/{sensors => systems/lines}/linesys_camera.h (96%) rename include/{sensors => systems/lines}/linesys_camera_new.h (100%) rename include/{ => systems}/position/positionsys_camera.h (93%) rename include/{ => systems}/position/positionsys_zone.h (99%) rename include/{position => systems}/systems.h (100%) rename src/strategy_roles/{goalie.cpp => striker.cpp} (89%) rename src/{sensors => system/lines}/linesys_2019.cpp (99%) rename src/{sensors => system/lines}/linesys_camera.cpp (94%) rename src/{sensors => system/lines}/linesys_camera_new.cpp (100%) rename src/{position => system/positions}/positionsys_camera.cpp (98%) rename src/{position => system/positions}/positionsys_zone.cpp (99%) create mode 100644 utility/OpenMV/conic_eff.py.autosave diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 3a67b28..b061937 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -1,7 +1,7 @@ #pragma once #include #include "strategy_roles/game.h" -#include "position/systems.h" +#include "systems/systems.h" /** * STATUS VECTOR: diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index 0c0f255..dd41ada 100644 --- a/include/sensors/sensors.h +++ b/include/sensors/sensors.h @@ -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" diff --git a/include/strategy_roles/game.h b/include/strategy_roles/game.h index b2822ce..81af76c 100644 --- a/include/strategy_roles/game.h +++ b/include/strategy_roles/game.h @@ -1,7 +1,7 @@ #pragma once #include "vars.h" -#include "position/systems.h" +#include "systems/systems.h" #include "sensors/sensors.h" class Game { diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 01c325c..3d55089 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -8,10 +8,10 @@ #include #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; \ No newline at end of file diff --git a/include/strategy_roles/goalie.h b/include/strategy_roles/striker.h similarity index 84% rename from include/strategy_roles/goalie.h rename to include/strategy_roles/striker.h index cefb904..6500e71 100644 --- a/include/strategy_roles/goalie.h +++ b/include/strategy_roles/striker.h @@ -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(); + void striker(); void ballBack(); void storcimentoPorta(); diff --git a/include/sensors/linesys_2019.h b/include/systems/lines/linesys_2019.h similarity index 96% rename from include/sensors/linesys_2019.h rename to include/systems/lines/linesys_2019.h index f8a7d3c..520ec31 100644 --- a/include/sensors/linesys_2019.h +++ b/include/systems/lines/linesys_2019.h @@ -3,7 +3,7 @@ #include #include "behaviour_control/ds_ctrl.h" -#include "position/systems.h" +#include "systems/systems.h" #include "vars.h" diff --git a/include/sensors/linesys_camera.h b/include/systems/lines/linesys_camera.h similarity index 96% rename from include/sensors/linesys_camera.h rename to include/systems/lines/linesys_camera.h index 333ea1d..d14708b 100644 --- a/include/sensors/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -3,7 +3,7 @@ #include #include "behaviour_control/ds_ctrl.h" -#include "position/systems.h" +#include "systems/systems.h" #include "vars.h" diff --git a/include/sensors/linesys_camera_new.h b/include/systems/lines/linesys_camera_new.h similarity index 100% rename from include/sensors/linesys_camera_new.h rename to include/systems/lines/linesys_camera_new.h diff --git a/include/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h similarity index 93% rename from include/position/positionsys_camera.h rename to include/systems/position/positionsys_camera.h index a5bb905..a099743 100644 --- a/include/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,12 +1,12 @@ #include "PID_v2.h" -#include "position/systems.h" +#include "systems/systems.h" //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 1 -#define CAMERA_CENTER_Y -10 +#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 diff --git a/include/position/positionsys_zone.h b/include/systems/position/positionsys_zone.h similarity index 99% rename from include/position/positionsys_zone.h rename to include/systems/position/positionsys_zone.h index 3600c83..5d42ddb 100644 --- a/include/position/positionsys_zone.h +++ b/include/systems/position/positionsys_zone.h @@ -1,6 +1,6 @@ #pragma once -#include "position/systems.h" +#include "systems/systems.h" //POSITION #define CENTERGOALPOST_VEL1 220 diff --git a/include/position/systems.h b/include/systems/systems.h similarity index 100% rename from include/position/systems.h rename to include/systems/systems.h diff --git a/src/main.cpp b/src/main.cpp index cd37a1e..dd0da0c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,7 +3,6 @@ #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" @@ -33,7 +32,7 @@ void loop() { updateSensors(); if(DEBUG.available()) testmenu->testMenu(); - goalie->play(role==1); + striker->play(role==1); keeper->play(role==0); // Last thing to do: movement and update status vector diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp index f9b5b05..2b51478 100644 --- a/src/strategy_roles/game.cpp +++ b/src/strategy_roles/game.cpp @@ -1,6 +1,6 @@ #include "behaviour_control/status_vector.h" #include "strategy_roles/game.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" Game::Game() {} Game::Game(LineSystem* ls_, PositionSystem* ps_) { diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 89d4a8f..8134766 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -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 lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; vector 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()); } \ No newline at end of file diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index 7e77f28..120ef0f 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -1,11 +1,11 @@ #include #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 "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" Keeper::Keeper() : Game() { @@ -36,7 +36,7 @@ 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; diff --git a/src/strategy_roles/goalie.cpp b/src/strategy_roles/striker.cpp similarity index 89% rename from src/strategy_roles/goalie.cpp rename to src/strategy_roles/striker.cpp index 9ef0f3e..591a641 100644 --- a/src/strategy_roles/goalie.cpp +++ b/src/strategy_roles/striker.cpp @@ -1,32 +1,32 @@ #include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" -#include "strategy_roles/goalie.h" +#include "strategy_roles/striker.h" #include "vars.h" #include "math.h" -Goalie::Goalie() : Game() { +Striker::Striker() : Game() { init(); } -Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { +Striker::Striker(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { init(); } -void Goalie::init(){ +void Striker::init(){ atk_speed = 0; atk_direction = 0; cstorc = 0; } -void Goalie::realPlay(){ - if(CURRENT_DATA_READ.ballSeen) this->goalie(); +void Striker::realPlay(){ + if(CURRENT_DATA_READ.ballSeen) this->striker(); else ((PositionSysCamera*)ps)->goCenter(); } -void Goalie::goalie() { +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; @@ -73,7 +73,7 @@ void Goalie::goalie() { -void Goalie::ballBack() { +void Striker::ballBack() { int ball_degrees2; int dir; @@ -91,7 +91,7 @@ void Goalie::ballBack() { } -void Goalie::storcimentoPorta() { +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; diff --git a/src/sensors/linesys_2019.cpp b/src/system/lines/linesys_2019.cpp similarity index 99% rename from src/sensors/linesys_2019.cpp rename to src/system/lines/linesys_2019.cpp index d8771d6..8ad3276 100644 --- a/src/sensors/linesys_2019.cpp +++ b/src/system/lines/linesys_2019.cpp @@ -1,4 +1,4 @@ -#include "sensors/linesys_2019.h" +#include "systems/lines/linesys_2019.h" #include "sensors/sensors.h" using namespace std; diff --git a/src/sensors/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp similarity index 94% rename from src/sensors/linesys_camera.cpp rename to src/system/lines/linesys_camera.cpp index 9f8bf83..5117329 100644 --- a/src/sensors/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -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; diff --git a/src/sensors/linesys_camera_new.cpp b/src/system/lines/linesys_camera_new.cpp similarity index 100% rename from src/sensors/linesys_camera_new.cpp rename to src/system/lines/linesys_camera_new.cpp diff --git a/src/position/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp similarity index 98% rename from src/position/positionsys_camera.cpp rename to src/system/positions/positionsys_camera.cpp index f6ee95c..52a074d 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -1,5 +1,5 @@ #include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" #include "vars.h" diff --git a/src/position/positionsys_zone.cpp b/src/system/positions/positionsys_zone.cpp similarity index 99% rename from src/position/positionsys_zone.cpp rename to src/system/positions/positionsys_zone.cpp index f9836ab..68fe989 100644 --- a/src/position/positionsys_zone.cpp +++ b/src/system/positions/positionsys_zone.cpp @@ -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" diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 1b10f00..4340afa 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -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(); diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index b04961c..2db0126 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (58, 97, 9, 45, 51, 92), # thresholds yellow goal - (12, 30, -25, 1, -49, -2)] # 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) @@ -64,8 +64,8 @@ sensor.set_saturation(3) sensor.set_brightness(0) sensor.set_quality(0) sensor.set_auto_whitebal(False) -sensor.set_auto_exposure(False, 4500) -sensor.set_auto_gain(False, 15) +sensor.set_auto_exposure(False, 5500) +sensor.set_auto_gain(True) sensor.skip_frames(time = 300) clock = time.clock() diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave new file mode 100644 index 0000000..1a7f5e3 --- /dev/null +++ b/utility/OpenMV/conic_eff.py.autosave @@ -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) +