EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

precision shooter: catches ball and goes towards goal 

depending on the angle it catches the ball at, sometimes it hides the ball, sometimes it does a random spinning kick when it founds the line
pull/1/head
EmaMaker 2021-06-22 10:39:14 +02:00
parent fc485a021a
commit f7a5b072df
5 changed files with 39 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/motors_movement/drivecontroller.h
View File

@ -10,7 +10,7 @@
//BEST NUMBERS YET
//USE MOVING AVERAGE AND ANGLE WRAP
#define KP 1.35
#define KP 1.2
#define KI 0.0
#define KD 0.025
@ -20,7 +20,7 @@
//Max possible vel 310
#define MAX_VEL 50
#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)
@ -37,6 +37,7 @@ class DriveController{
void drive(int dir=0, int speed=0, int tilt=0);
void prepareDrive(int dir, int speed, int tilt=0);
void drivePrepared();
int directionAccountingForTilt(int, int);
float updatePid();
float torad(float f);
void resetDrive();

2
include/sensors/data_source_camera_conicmirror.h
View File

@ -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 -5
#define CAMERA_TRANSLATION_X 0
#define CAMERA_TRANSLATION_Y 0
class DataSourceCameraConic : public DataSource{

4
src/motors_movement/drivecontroller.cpp
View File

@ -167,4 +167,8 @@ void DriveController::stopAll(){
m2->stop();
m3->stop();
m4->stop();
}
int DriveController::directionAccountingForTilt(int wanted_direction, int tilt){
return ((360-tilt+wanted_direction)+360)%360;
}

51
src/strategy_roles/precision_shooter.cpp
View File

@ -29,12 +29,28 @@ void PrecisionShooter::init()
ballAngleFilter = new ComplementaryFilter(0.85);
}
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->spinner(5);
else
this->catchBall();
else{
ps->goCenter();
ball_catch_flag = false;
spinner_flag = false;
ball_catch_state = 0;
spinner_state = 0;
}
}
/*
@ -43,13 +59,6 @@ Spinning kick state machine
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
void PrecisionShooter::spinner(int targetx){
// if(!ballPresence->isInMouth()) {
// spinner_state=0;
@ -128,10 +137,6 @@ Ball catch state machine
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
void PrecisionShooter::catchBall(){
@ -144,27 +149,29 @@ void PrecisionShooter::catchBall(){
}
if(ball_catch_state == 0){
drive->prepareDrive(0, 30, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 250) {
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
drive->prepareDrive(0,0,ball_catch_tilt);
// if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
// else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
}else if(ball_catch_state == 2){
((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
// drive->prepareDrive(0,0,ball_catch_tilt);
// if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
// ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
}
}

4
utility/OpenMV/conic_eff_h7.py
View File

@ -46,10 +46,10 @@ blue_led.on()
thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz
(31, 74, -31, 15, -50, -9)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
(37, 57, -19, 8, -52, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (50, 0, 250, 200)
roi = (50, 0, 270, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx