diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index b80adfc..5c70039 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -36,7 +36,7 @@ typedef struct input{ }input; typedef struct data{ - int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal, + int IMUAngle = 0, IMUOffset = 0, ballAngle = 0, ballAngleFix, ballDistance, ballPresenceVal, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix, diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index a98706b..6e6c60f 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -22,7 +22,7 @@ // #define MAX_POSSIBLE_VEL 310 #define MAX_POSSIBLE_VEL 280 -#define MAX_VEL 80 +#define MAX_VEL 60 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8) #define MAX_VEL_HALF ((int)MAX_VEL*0.5) #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 449aa75..8fbff0d 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -27,7 +27,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/ // #define CAMERA_TRANSLATION_Y 7 //Robot with roller -#define CAMERA_TRANSLATION_X -3 +#define CAMERA_TRANSLATION_X 0 #define CAMERA_TRANSLATION_Y -3 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/round_robin.h b/include/strategy_roles/round_robin.h index b039a72..6794300 100644 --- a/include/strategy_roles/round_robin.h +++ b/include/strategy_roles/round_robin.h @@ -13,12 +13,12 @@ // There needs to be a little bit of space between the target point and the spot to be in #define RR_SPINNER_OVERHEAD 7 -#define RR_KICK_LIMIT_TILT1 135 -#define RR_KICK_LIMIT_MAX 335 -#define RR_KICK_LIMIT_MIN 25 +#define RR_KICK_LIMIT_TILT1 200 +#define RR_KICK_LIMIT_MAX 315 +#define RR_KICK_LIMIT_MIN 45 #define ROUND_ROBIN_VEL 30 -#define RR_YCOORD -8 +#define RR_YCOORD -5 #define RR_ROLLER_SPD 1500 class RoundRobin : public Game{ @@ -32,6 +32,8 @@ class RoundRobin : public Game{ void init() override; void catchBall(); void spinner(int); + void push(); + void spinnerStep(); private: int atk_speed, atk_direction, atk_tilt; @@ -39,6 +41,8 @@ class RoundRobin : public Game{ bool gotta_tilt; ComplementaryFilter* ballAngleFilter; + int flip = 0; + float tilt1 = 0; float tilt2 = 0; int spinner_state = 0; diff --git a/include/systems/lines/linesys_camera_roundrobin.h b/include/systems/lines/linesys_camera_roundrobin.h new file mode 100644 index 0000000..5e1ea9d --- /dev/null +++ b/include/systems/lines/linesys_camera_roundrobin.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +#include "behaviour_control/ds_ctrl.h" +#include "systems/systems.h" + +#include "vars.h" + +#define S1O A7 +#define S1I A6 +#define S2O A2 +#define S2I A3 +#define S3I A9 +#define S3O A8 +#define S4I A0 +#define S4O A1 + +#define LINE_THRESH_CAM 350 +#define EXIT_TIME 300 + +class LineSysCameraRR : public LineSystem{ + + public: + LineSysCameraRR(); + LineSysCameraRR(vector in_, vector out); + + void update() override; + void test() override; + void outOfBounds(); + void checkLineSensors(); + + public: + bool tookLine = false, flag = false; + int linetriggerI[4], linetriggerO[4]; + + private: + vector in, out; + DataSource* ds; + int inV, outV, linesensOldX, linesensOldY, value, linepins[4], i; +}; \ No newline at end of file diff --git a/src/sensors/data_source_bno055.cpp b/src/sensors/data_source_bno055.cpp index 340dce1..5e86a39 100644 --- a/src/sensors/data_source_bno055.cpp +++ b/src/sensors/data_source_bno055.cpp @@ -18,6 +18,7 @@ void DataSourceBNO055::readSensor(){ if(millis() - lastTime > DATA_CLOCK){ imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); this->value = (int) euler.x(); + this->value = (this->value+CURRENT_DATA_READ.IMUOffset+360)%360; lastTime = millis(); CURRENT_INPUT_WRITE.IMUAngle = this->value; CURRENT_DATA_WRITE.IMUAngle = this->value; diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index d57185e..1b0326c 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -2,6 +2,7 @@ /* #include "sensors/linesys_2019.h" */ #include "systems/lines/linesys_camera.h" +#include "systems/lines/linesys_camera_roundrobin.h" #include "systems/systems.h" #include "systems/position/positionsys_zone.h" #include "systems/position/positionsys_camera.h" @@ -17,7 +18,7 @@ void initGames(){ precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera()); striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera()); tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera()); - tc2 = new RoundRobin(new LineSystemEmpty(), new PositionSysCamera()); + tc2 = new RoundRobin(new LineSysCameraRR(lIn, lOut), new PositionSysCamera()); st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera()); st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera()); // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); diff --git a/src/strategy_roles/round_robin.cpp b/src/strategy_roles/round_robin.cpp index 493fb62..22de3c6 100644 --- a/src/strategy_roles/round_robin.cpp +++ b/src/strategy_roles/round_robin.cpp @@ -1,5 +1,6 @@ #include "behaviour_control/status_vector.h" #include "systems/position/positionsys_camera.h" +#include "systems/lines/linesys_camera_roundrobin.h" #include "sensors/sensors.h" #include "sensors/data_source_ball.h" #include "strategy_roles/round_robin.h" @@ -33,6 +34,8 @@ void RoundRobin::realPlay() { if (CURRENT_DATA_READ.ballSeen) this->catchBall(); + // this->spinner(0); + // this->push(); else{ ps->goCenter(); ball_catch_flag = false; @@ -49,19 +52,16 @@ Spinning kick state machine 2: tilt back to 0 in the needed direction, stopping the roller whenn needed */ void RoundRobin::spinner(int targetx){ - // if(!ballPresence->isInMouth()) { - // spinner_state=0; - // spinner_flag = false; - // } if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX-200); + if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); if(ballPresence->isInMouth() && !spinner_flag){ spinner_flag = true; spinner_timer = millis(); } - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 100) { + + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { spinner_state=1; spinner_flag = false; } @@ -72,34 +72,34 @@ void RoundRobin::spinner(int targetx){ if(targetx >= 0) spotx = targetx-RR_SPINNER_OVERHEAD; else spotx = targetx+RR_SPINNER_OVERHEAD; - if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, RR_YCOORD)) { - // if( !spinner_flag){ - // spinner_flag = true; - // spinner_timer = millis(); - // } + // if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) { - // if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { + // if( !spinner_flag){ + // spinner_flag = true; + // spinner_timer = millis(); + // } + + // if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { spinner_state=2; - spinner_flag = false; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - // } + // spinner_flag = false; + // spinner_tilt = CURRENT_DATA_READ.IMUAngle; + // } - if(targetx >= 0) { + // if(targetx >= 0) { tilt1 = -0.15; - tilt2 = -1.55; - limitx = RR_KICK_LIMIT_TILT1; + tilt2 = -35; - }else{ - tilt1 = 0.15; - tilt2 = 1.55; limitx = 360-RR_KICK_LIMIT_TILT1; + limitx = 360-RR_KICK_LIMIT_TILT1; + // }else{ + // tilt1 = 0.15; + // tilt2 = -0.55; - - } + // limitx = RR_KICK_LIMIT_TILT1; + // } - }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, RR_YCOORD); - + // }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0); }else if(spinner_state == 2){ - roller->speed(RR_ROLLER_SPD); + roller->speed(roller->MAX); spinner_tilt += tilt1; drive->prepareDrive(0,0,spinner_tilt); @@ -109,7 +109,7 @@ void RoundRobin::spinner(int targetx){ spinner_state=3; } }else if(spinner_state == 3){ - roller->speed(RR_ROLLER_SPD); + roller->speed(roller->MAX); drive->prepareDrive(0,0,spinner_tilt); if(millis() - spinner_timer > 150) { spinner_state=4; @@ -117,17 +117,145 @@ void RoundRobin::spinner(int targetx){ } }else if(spinner_state == 4){ // drive->prepareDrive(0,0,0); - spinner_tilt += tilt2; spinner_tilt = constrain(spinner_tilt, RR_KICK_LIMIT_MIN, RR_KICK_LIMIT_MAX); drive->prepareDrive(0,0,spinner_tilt); if(CURRENT_DATA_READ.IMUAngle >= RR_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= RR_KICK_LIMIT_MIN) { - roller->speed(roller->MIN); - spinner_tilt = 0; + ball_catch_state++; + + roller->speed(roller->MIN); } } } + + +void RoundRobin::spinnerStep(){ + spinner_state++; + spinner_timer = millis(); +} + +/*void RoundRobin::push(){ + if(spinner_state == 0){ + if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); + + if(ballPresence->isInMouth() && !spinner_flag){ + spinner_flag = true; + spinner_timer = millis(); + } + + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { + spinner_state=1; + spinner_flag = false; + } + }else if(spinner_state == 1){ + roller->speed(roller->MAX); + + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -6)) spinner_state=4; + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -6); + // } else if(spinner_state == 2){ + // roller->speed(roller->MAX); + // if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep(); + // else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4); + // }else if(spinner_state == 3){ + // drive->prepareDrive(0,0,0); + // if(millis() - spinner_timer > 750) spinnerStep(); + }else if(spinner_state == 4){ + drive->prepareDrive(45, 50, 0); + if( ((LineSysCameraRR*)ls)->tookLine ) spinnerStep(); + }else if(spinner_state == 5){ + roller->speed(roller->MIN); + if(millis()-spinner_timer > 1500) spinnerStep(); + }else if(spinner_state == 6){ + drive->prepareDrive(180, 30, 0); + if(millis() - spinner_timer > 750) spinnerStep(); + }else if(spinner_state == 7){ + roller->speed(roller->MIN); + drive->prepareDrive(0, 0, 55); + if(millis() - spinner_timer > 1000) spinnerStep(); + }else if(spinner_state == 8){ + drive->prepareDrive(drive->directionAccountingForTilt(0, 55), 100, 55); + if(millis() - spinner_timer > 500) spinnerStep(); + }else if(spinner_state == 9){ + drive->prepareDrive(drive->directionAccountingForTilt(180, 55), 50, 55); + if(millis() - spinner_timer > 750) spinnerStep(); + }else if(spinner_state == 10){ + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -8)) spinnerStep(); + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -8); + }else ball_catch_state++; +}*/ +void RoundRobin::push(){ + if(spinner_state == 0){ + if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); + + if(ballPresence->isInMouth() && !spinner_flag){ + spinner_flag = true; + spinner_timer = millis(); + } + + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { + spinner_state=1; + spinner_flag = false; + } + }else if(spinner_state == 1){ + roller->speed(roller->MAX); + + // if(flip) + // if(CURRENT_DATA_READ.yDist <= 24) drive->prepareDrive(0, 50, 0); + // if(CURRENT_DATA_READ.yDist >= 36) drive->prepareDrive(180, 50, 0); + // else{ + // if(CURRENT_DATA_READ.bDist <= 24) drive->prepareDrive(0, 50, 0); + // if(CURRENT_DATA_READ.bDist >= 36) drive->prepareDrive(180, 50, 0); + // } + + if(flip==0){ + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -3)) spinnerStep(); + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -3); + }else{ + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -12)) spinnerStep(); + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -12); + } + } else if(spinner_state == 2){ + // roller->speed(roller->MAX); + // if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep(); + // else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4); + // }else if(spinner_state == 3){ + // drive->prepareDrive(0,0,0); + // if(millis() - spinner_timer > 750) spinnerStep(); + // }else if(spinner_state == 4){ + // drive->prepareDrive(45, 50, 0); + drive->prepareDrive(70, 40, 0); + if( ((LineSysCameraRR*)ls)->tookLine ) { + spinner_state = 5; + spinner_timer = millis(); + } + }else if(spinner_state == 5){ + roller->speed(roller->MIN); + if(millis()-spinner_timer > 1500) spinnerStep(); + }else if(spinner_state == 6){ + drive->prepareDrive(180, 30, 0); + if(millis() - spinner_timer > 650) spinnerStep(); + }else if(spinner_state == 7){ + roller->speed(roller->MIN); + drive->prepareDrive(0, 0, 55); + if(millis() - spinner_timer > 1000) spinnerStep(); + }else if(spinner_state == 8){ + drive->prepareDrive(drive->directionAccountingForTilt(0, 90), 100, 90); + if(millis() - spinner_timer > 400) spinnerStep(); + }else if(spinner_state == 9){ + drive->prepareDrive(drive->directionAccountingForTilt(180, 90), 50, 90); + if(millis() - spinner_timer > 750) spinnerStep(); + }else if(spinner_state == 10){ + if(flip==0){ + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -4)) spinnerStep(); + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -4); + }else{ + if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -13)) spinnerStep(); + else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -13); + } + }else ball_catch_state++; +} + /* Ball catch state machine 0: go towards the ball, until it's been in robot's mouth for 250ms @@ -137,13 +265,14 @@ Ball catch state machine void RoundRobin::catchBall(){ - if(!ball->isInFront()){ - ball_catch_state = 0; - ball_catch_flag = false; - ball_catch_tilt = 0; - } + // if(!ball->isInFront() && ball_catch_state != 3){ + // ball_catch_state = 0; + // ball_catch_flag = false; + // ball_catch_tilt = 0; + // } if(ball_catch_state == 0){ + ((LineSysCameraRR*)ls)->flag = true; if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); else roller->speed(roller->MIN); @@ -172,7 +301,24 @@ void RoundRobin::catchBall(){ spinner_timer = 0; spinner_state = 1; ball_catch_state = 3; + + ((LineSysCameraRR*)ls)->flag = false; }else if(ball_catch_state == 3){ - this->spinner(28); - } + // this->spinner(25); + this->push(); + }else if(ball_catch_state == 4){ + drive->prepareDrive(270, 50, 0); + if(((LineSysCameraRR*)ls)->tookLine && CURRENT_DATA_READ.posx <= -10) ball_catch_state = 5; + }else if(ball_catch_state == 5){ + if( ((LineSysCameraRR*)ls)->linetriggerI[1] || ((LineSysCameraRR*)ls)->linetriggerO[1] > 0) drive->prepareDrive(90, 50, 0); //with 2 and 4 you go right or left accordingly + else if( ((LineSysCameraRR*)ls)->linetriggerI[3] || ((LineSysCameraRR*)ls)->linetriggerO[3] > 0 ) drive->prepareDrive(270, 50, 0); + else drive->prepareDrive(0, 30, 0); + if(CURRENT_DATA_READ.posy > 7) { + ball_catch_state=0; + + flip = 1-flip; + + CURRENT_DATA_WRITE.IMUOffset = 180 * flip; + } + }else if(ball_catch_state==6) drive->prepareDrive(0,0,0); } diff --git a/src/system/lines/linesys_camera_roundrobin.cpp b/src/system/lines/linesys_camera_roundrobin.cpp new file mode 100644 index 0000000..3d53554 --- /dev/null +++ b/src/system/lines/linesys_camera_roundrobin.cpp @@ -0,0 +1,77 @@ +#include "systems/lines/linesys_camera_roundrobin.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "strategy_roles/games.h" +#include "behaviour_control/status_vector.h" + +LineSysCameraRR::LineSysCameraRR(){} +LineSysCameraRR::LineSysCameraRR(vector in_, vector out_){ + this->in = in_; + this->out = out_; + + linesensOldX = 0; + linesensOldY = 0; + + tookLine = false; + + for(int i = 0; i < 4; i++){ + linetriggerI[i] = 0; + linetriggerO[i] = 0; + } +} + + +void LineSysCameraRR ::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); + } + + tookLine = inV > 0 || outV > 0; + // if(flag && tookLine) ((PositionSysCamera*)CURRENT_DATA_READ.posSystem)->setMoveSetpoints(0, -8); +} + +void LineSysCameraRR::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/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 9e88e4e..6a8358f 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -152,7 +152,7 @@ void PositionSysCamera::CameraPID(){ dir = (dir+360) % 360; float distance = hypot(Outputx, Outputy); - float speed = distance > 3 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_POSSIBLE_VEL) : 0; + float speed = distance > 2 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, 200) : 0; // DEBUG.print("x: "); // DEBUG.print(Outputx); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index c79be2c..1cdf301 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,11 +45,11 @@ blue_led.on() ############################################################################## -thresholds = [ (68, 100, -16, 26, 24, 85), # thresholds yellow goalz - (36, 70, -3, 27, -75, -27)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (62, 100, -10, 21, 27, 112), # thresholds yellow goalz + (34, 60, -4, 18, -65, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -roi = (70, 0, 250, 200) +roi = (50, 0, 270, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx @@ -71,7 +71,7 @@ sensor.set_windowing(roi) sensor.set_contrast(0) sensor.set_saturation(2) sensor.set_brightness(3) -sensor.set_auto_whitebal(True, (-6.02073, -5.119987, -1.006964)) +sensor.set_auto_whitebal(False, (-6.02073, -4.99849, -1.083474)) sensor.set_auto_exposure(False, 6576) #sensor.set_auto_gain(False, gain_db=8.78) sensor.skip_frames(time = 300)