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

robocoup: tc2 - round robin

pull/1/head
EmaMaker 2021-06-25 08:41:30 +02:00
parent 77218487c9
commit f926ecf027
11 changed files with 320 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/behaviour_control/status_vector.h
View File

@ -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,

2
include/motors_movement/drivecontroller.h
View File

@ -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)

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

12
include/strategy_roles/round_robin.h
View File

@ -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;

41
include/systems/lines/linesys_camera_roundrobin.h Normal file
View File

@ -0,0 +1,41 @@
#pragma once
#include <Arduino.h>
#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<DataSource*> in_, vector<DataSource*> out);
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
public:
bool tookLine = false, flag = false;
int linetriggerI[4], linetriggerO[4];
private:
vector<DataSource*> in, out;
DataSource* ds;
int inV, outV, linesensOldX, linesensOldY, value, linepins[4], i;
};

1
src/sensors/data_source_bno055.cpp
View File

@ -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;

3
src/strategy_roles/games.cpp
View File

@ -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());

220
src/strategy_roles/round_robin.cpp
View File

@ -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);
}

77
src/system/lines/linesys_camera_roundrobin.cpp Normal file
View File

@ -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<DataSource*> in_, vector<DataSource*> 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();
}

2
src/system/positions/positionsys_camera.cpp
View File

@ -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);

8
utility/OpenMV/conic_eff_h7.py
View File

@ -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)