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

striker-roller: complete striker using the roller

pull/1/head
EmaMaker 2021-06-22 18:18:00 +02:00
parent c168d0a1b1
commit 5c085bb651
9 changed files with 263 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/behaviour_control/status_vector.h
View File

@ -39,7 +39,7 @@ typedef struct data{
int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal,
yAngle, bAngle, yAngleFix, bAngleFix,
yDist, bDist,
angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
cam_xb, cam_yb, cam_xy, cam_yy,
speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr,

2
include/strategy_roles/games.h
View File

@ -9,6 +9,7 @@
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "strategy_roles/striker.h"
#include "strategy_roles/striker_roller.h"
#include "strategy_roles/precision_shooter.h"
#include "strategy_roles/pass_and_shoot.h"
// #include "strategy_roles/keeper.h"
@ -16,6 +17,7 @@
void initGames();
g_extr Game* striker;
g_extr Game* striker_roller;
g_extr Game* precision_shooter;
g_extr Game* pass_and_shoot;
// g_extr Game* keeper;

18
include/strategy_roles/precision_shooter.h
View File

@ -13,9 +13,9 @@
// There needs to be a little bit of space between the target point and the spot to be in
#define PS_SPINNER_OVERHEAD 7
#define KICK_LIMIT_TILT1 200
#define KICK_LIMIT_MAX 315
#define KICK_LIMIT_MIN 45
#define PS_KICK_LIMIT_TILT1 200
#define PS_KICK_LIMIT_MAX 315
#define PS_KICK_LIMIT_MIN 45
class PrecisionShooter : public Game{
@ -35,4 +35,16 @@ class PrecisionShooter : public Game{
float cstorc;
bool gotta_tilt;
ComplementaryFilter* ballAngleFilter;
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;
int limitx = 0;
};

51
include/strategy_roles/striker_roller.h Normal file
View File

@ -0,0 +1,51 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "behaviour_control/complementary_filter.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define STRL_ATTACK_DISTANCE 110
#define STRL_TILT_STOP_DISTANCE 140
#define STRL_PLUSANG 55
#define STRL_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define STRL_SPINNER_OVERHEAD 7
#define STRL_KICK_LIMIT_TILT1 200
#define STRL_KICK_LIMIT_MAX 315
#define STRL_KICK_LIMIT_MIN 45
class StrikerRoller : public Game{
public:
StrikerRoller();
StrikerRoller(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void catchBall();
void spinner(int);
int tilt();
private:
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
ComplementaryFilter* ballAngleFilter;
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;
int limitx = 0;
};

2
src/main.cpp
View File

@ -53,7 +53,7 @@ void loop() {
// striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
// keeper_condition = role == LOW;
precision_shooter->play(1);
striker_roller->play(1);
testmenu->testMenu();

4
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -160,6 +160,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.yDist;
CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed;
@ -169,6 +170,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.bDist;
CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed;
@ -178,6 +180,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.bDist;
CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed;
@ -187,6 +190,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.yDist;
CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed;

1
src/strategy_roles/games.cpp
View File

@ -15,5 +15,6 @@ void initGames(){
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera());
precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
// keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

20
src/strategy_roles/precision_shooter.cpp
View File

@ -29,18 +29,6 @@ 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;
int limitx = 0;
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
@ -103,12 +91,12 @@ void PrecisionShooter::spinner(int targetx){
tilt1 = -0.15;
tilt2 = 0.55;
limitx = 360-KICK_LIMIT_TILT1;
limitx = 360-PS_KICK_LIMIT_TILT1;
}else{
tilt1 = 0.15;
tilt2 = -0.55;
limitx = KICK_LIMIT_TILT1;
limitx = PS_KICK_LIMIT_TILT1;
}
}else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
@ -133,10 +121,10 @@ void PrecisionShooter::spinner(int targetx){
drive->prepareDrive(0,0,0);
// spinner_tilt += tilt2;
// spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
// spinner_tilt = constrain(spinner_tilt, PS_KICK_LIMIT_MIN, PS_KICK_LIMIT_MAX);
// drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle >= KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= KICK_LIMIT_MIN) roller->speed(roller->MIN);
if(CURRENT_DATA_READ.IMUAngle >= PS_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= PS_KICK_LIMIT_MIN) roller->speed(roller->MIN);
}
}
/*

183
src/strategy_roles/striker_roller.cpp Normal file
View File

@ -0,0 +1,183 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/striker_roller.h"
#include "vars.h"
#include "math.h"
StrikerRoller::StrikerRoller() : Game()
{
init();
}
StrikerRoller::StrikerRoller(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void StrikerRoller::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
ballAngleFilter = new ComplementaryFilter(0.85);
}
void StrikerRoller::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->catchBall();
else{
ps->goCenter();
ball_catch_flag = false;
spinner_flag = false;
ball_catch_state = 0;
spinner_state = 0;
}
}
/*
Spinning kick state machine
0: wait for the ball to be in mouth, calculate movement direction
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
void StrikerRoller::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);
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(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=2;
spinner_flag = false;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
if(CURRENT_DATA_READ.posx < targetx) {
tilt1 = -0.15;
tilt2 = 0.55;
limitx = 360-STRL_KICK_LIMIT_TILT1;
}else{
tilt1 = 0.15;
tilt2 = -0.55;
limitx = STRL_KICK_LIMIT_TILT1;
}
spinner_state=1;
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt += tilt1;
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
spinner_timer = millis();
spinner_state=3;
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
drive->prepareDrive(0,0,spinner_tilt);
if(millis() - spinner_timer > 150) {
spinner_state=4;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 4){
drive->prepareDrive(0,0,0);
// spinner_tilt += tilt2;
// spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
// drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) roller->speed(roller->MIN);
}
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
void StrikerRoller::catchBall(){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
else roller->speed(roller->MIN);
if(!ball->isInFront()){
ball_catch_state = 0;
ball_catch_flag = false;
}
if(ball_catch_state == 0){
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 > 350) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
if(CURRENT_DATA_READ.distAtk <= 30){
ball_catch_state++;
spinner_state = 0;
spinner_flag = false;
spinner_tilt = 0;
drive->prepareDrive(0,0,0);
}
}else if(ball_catch_state == 2){
spinner(0);
}
// if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
// else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
// 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++;
}
int StrikerRoller::tilt() {
if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
atk_tilt *= 0.8;
if(atk_tilt <= 10) atk_tilt = 0;
}else{
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;
}