diff --git a/include/motors_movement/roller.h b/include/motors_movement/roller.h index ff0fbb6..d7242b5 100644 --- a/include/motors_movement/roller.h +++ b/include/motors_movement/roller.h @@ -2,7 +2,7 @@ #include "ESC.h" -#define ROLLER_DEFAULT_SPEED 1300 +#define ROLLER_DEFAULT_SPEED 1100 class Roller{ public: diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 1766cc5..2f76213 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -7,7 +7,7 @@ #define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_TILT_STOP_DISTANCE 140 #define STRIKER_PLUSANG 55 -#define STRIKER_PLUSANG_VISIONCONE 10 +#define STRIKER_PLUSANG_VISIONCONE 7 class Striker : public Game{ diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index 7bb1a18..b128b26 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -16,7 +16,7 @@ #define S4I A9 #define S4O A8 -#define LINE_THRESH_CAM 350 +#define LINE_THRESH_CAM 325 #define EXIT_TIME 250 #define LINES_EXIT_SPD 350 diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 31cddb0..018cb50 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -8,7 +8,7 @@ To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. These values need to be subtracted from the coords used in setMoveSetpoints*/ #define CAMERA_TRANSLATION_X 4 -#define CAMERA_TRANSLATION_Y 4 +#define CAMERA_TRANSLATION_Y 12 //Camera center: those setpoints correspond to what we consider the center of the field #define CAMERA_CENTER_X 0 #define CAMERA_CENTER_Y 0 diff --git a/include/vars.h b/include/vars.h index cc9c3f9..3a01874 100755 --- a/include/vars.h +++ b/include/vars.h @@ -21,4 +21,6 @@ in the 32U4 code*/ #define ROLLER_INA 34 #define ROLLER_INB 35 +#define BALL_32U4 Serial2 + extr float sins[360], cosins[360]; diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index f3dd7db..f8eea5d 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int 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])) + CURRENT_DATA_READ.addvx; + //vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; - // vx = ((speed * cosins[dir])); - // vy = ((-speed * sins[dir])); + 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; diff --git a/src/motors_movement/roller.cpp b/src/motors_movement/roller.cpp index 549836f..c107f9f 100644 --- a/src/motors_movement/roller.cpp +++ b/src/motors_movement/roller.cpp @@ -91,5 +91,6 @@ void Roller::update(){ } void Roller::speed(int speed){ - roller->speed(speed); + if(roller_armed) + roller->speed(speed); } \ No newline at end of file diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 13ca81d..2569c81 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -1,5 +1,6 @@ #include "behaviour_control/status_vector.h" #include "sensors/data_source_camera_conicmirror.h" +#include "vars.h" //Comment out to disable complementary filters on angles #define CAMERA_CONIC_FILTER_POINTS @@ -170,6 +171,11 @@ void DataSourceCameraConic ::computeCoordsAngles() { CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix; CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen; } + + byte to_32u4 = 0; + to_32u4 |= (CURRENT_DATA_READ.ySeen); + to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1; + BALL_32U4.write(to_32u4); } void DataSourceCameraConic::test(){ diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index 96ee33e..e2acb22 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -8,7 +8,7 @@ void initSensors(){ drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305)); compass = new DataSourceBNO055(); - ball = new DataSourceBall(&Serial2, 57600); + ball = new DataSourceBall(&BALL_32U4, 57600); camera = new DataSourceCameraConic(&Serial3, 19200); bt = new DataSourceBT(&Serial1, 115200); roller = new Roller(30, 31, 1000, 2000, 500); diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index a5b76ec..d53e226 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -35,38 +35,24 @@ void Striker::realPlay() ps->goCenter(); } -void Striker::striker() -{ - int plusang = STRIKER_PLUSANG, ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle; +void Striker::striker(){ + //seguo palla + int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG; + + if(ball_deg >= 346 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus + if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180 + else ball_degrees2 = ball_deg; - if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE) - { - drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_HALF, 0); - return; - } + if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo + else dir = ball_deg - plusang; //se sto nel negativo sottraggo - if (ball_deg > 340 || ball_deg < 20) - plusang -= STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus - if (ball_deg > 180) - ball_degrees2 = ball_deg - 360; //ragiono in +180 -180 - else - ball_degrees2 = ball_deg; - - if (ball_degrees2 > 0) - dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo - else - dir = ball_deg - plusang; //se sto nel negativo sottraggo - - if (dir < 0) - dir = dir + 360; //se sto nel quadrante negativo ricappotto - else - dir = dir; + dir = (dir + 360) % 360; drive->prepareDrive(dir, MAX_VEL_HALF, tilt()); } int Striker::tilt() { - if (ball->isInMouth() || (ball->isInMouthMaxDistance() && gotta_tilt)) gotta_tilt = true; + if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true; else gotta_tilt = false; if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) { diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index f49ad28..ec8bb81 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -126,18 +126,18 @@ void PositionSysCamera::CameraPID(){ speed = filterSpeed->calculate(speed); speed = speed > 40 ? speed : 0; dir = filterDir->calculate(dir); - // drive->prepareDrive(dir, speed, 0); + 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; } } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 4544cdf..ed3407d 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,11 +45,11 @@ blue_led.on() ############################################################################## -thresholds = [ (48, 83, -9, 18, 38, 82), # thresholds yellow goal - (45, 61, -11, 14, -42, -22)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (55, 92, -3, 24, 60, 90), # thresholds yellow goal + (33, 49, -9, 12, -52, -12)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -roi = (20, 0, 300, 220) +roi = (30, 0, 290, 240) # Camera Setup ############################################################### '''sensor.reset()xxxx @@ -94,7 +94,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=50, area_threshold=70, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=90, merge = True): img.draw_rectangle(blob.rect()) #img.draw_cross(blob.cx(), blob.cy())