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

precision shooter,camera: improve, fix math error in translation 

alongside with better speed calculation and utility methods
pull/1/head
EmaMaker 2021-06-21 11:17:33 +02:00
parent c425472f58
commit 827e063155
13 changed files with 62 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/motors_movement/drivecontroller.h
View File

@ -20,7 +20,7 @@
//Max possible vel 310
#define MAX_VEL 120
#define MAX_VEL 50
#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_ball_presence.h
View File

@ -1,6 +1,6 @@
#pragma once
#define BALL_PRESENCE_TRESH 500
#define BALL_PRESENCE_TRESH 600
#include <Arduino.h>
#include "behaviour_control/data_source.h"

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

4
include/strategy_roles/precision_shooter.h
View File

@ -11,7 +11,7 @@
#define PS_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define PS_SPINNER_OVERHEAD 10
#define PS_SPINNER_OVERHEAD 6
class PrecisionShooter : public Game{
@ -23,7 +23,7 @@ class PrecisionShooter : public Game{
void realPlay() override;
void init() override;
void catchBall();
void spinner();
void spinner(int);
int tilt();
private:

4
include/systems/position/positionsys_camera.h
View File

@ -20,9 +20,9 @@
//Actually it's ± MAX_VAL
#define MAX_X 50
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 1.7
#define DIST_MULT 3.5
#define VICINITY_DIST_TRESH 3
#define VICINITY_DIST_TRESH 2
#define Kpx 1
#define Kix 0

2
src/main.cpp
View File

@ -57,6 +57,8 @@ void loop() {
testmenu->testMenu();
// if(roller->roller_armed) roller->speed(roller->MAX);
// Last thing to do: movement and update status vector
drive->drivePrepared();
updateStatusVector();

2
src/sensors/data_source_ball_presence.cpp
View File

@ -25,5 +25,5 @@ void DataSourceBallPresence::test(){
DEBUG.print(" -> ");
DEBUG.print(CURRENT_DATA_READ.ballPresent);
DEBUG.print(" | Ball in mouth: ");
DEBUG.print(isInMouth());
DEBUG.println(isInMouth());
}

8
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -87,10 +87,10 @@ void DataSourceCameraConic ::readSensor() {
void DataSourceCameraConic ::computeCoordsAngles() {
//Where are the goals relative to the robot?
//Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera
true_xb = 50 - true_xb + CAMERA_TRANSLATION_X;
true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y;
true_xy = 50 - true_xy + CAMERA_TRANSLATION_X;
true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y;
true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5;
true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y*0.5;
true_xy = 50 - true_xy + CAMERA_TRANSLATION_X*0.5;
true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y*0.5;
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb = filter_xb->calculate(true_xb);

2
src/sensors/sensors.cpp
View File

@ -20,7 +20,7 @@ void initSensors(){
// delay(350);
bt = new DataSourceBT(&Serial1, 9600);
roller = new Roller(30, 31, 1000, 2000, 500);
ballPresence = new DataSourceBallPresence(A22, true);
ballPresence = new DataSourceBallPresence(A13, true);
}
void updateSensors(){

64
src/strategy_roles/precision_shooter.cpp
View File

@ -32,7 +32,7 @@ void PrecisionShooter::init()
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->catchBall();
this->spinner(0);
else
ps->goCenter();
}
@ -51,61 +51,75 @@ unsigned long spinner_timer = 0;
float spinner_tilt = 0;
void PrecisionShooter::spinner(int targetx){
if(!ballPresence->isInMouth()) {
spinner_state=0;
spinner_flag = false;
}
// if(!ballPresence->isInMouth()) {
// spinner_state=0;
// spinner_flag = false;
// }
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
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 > 250) {
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state++;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
spinner_flag = false;
}
}else if(spinner_state == 1){
int spotx;
roller->speed(roller->MAX);
int spotx;
if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD;
else spotx = targetx+PS_SPINNER_OVERHEAD;
((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)){
ball_catch_state++;
if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 5)) {
if( !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 750) {
spinner_state++;
spinner_flag = false;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
if(CURRENT_DATA_READ.posx > targetx){
tilt1 = -0.1;
tilt2 = 0.35;
tilt1 = -0.075;
tilt2 = 0.3;
}else{
tilt1 = 0.1;
tilt2 = -0.35;
tilt1 = 0.075;
tilt2 = -0.3;
}
}
}else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 5);
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt += tilt1;
drive(0,0,spinner_tilt);
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) {
spinner_state++;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
spinner_timer = millis();
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
if(millis() - spinner_timer > 2000) spinner_state++;
}else if(spinner_state == 4){
spinner_tilt += tilt2;
drive(0,0,spinner_tilt);
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > 225 || CURRENT_DATA_READ.IMUAngle < 135){
roller->speed(roller->MIN);
spinner_state++;
}
if(CURRENT_DATA_READ.IMUAngle > 215 || CURRENT_DATA_READ.IMUAngle < 125) roller->speed(roller->MIN);
if(CURRENT_DATA_READ.IMUAngle > 355 || CURRENT_DATA_READ.IMUAngle < 10) spinner_state++;
}
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms

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

@ -145,10 +145,10 @@ void PositionSysCamera::CameraPID(){
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
int speed = dist*DIST_MULT;
speed = speed > 10 ? speed : 0;
// int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
// int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
int speed = hypot(Outputx, Outputy) * DIST_MULT;
// speed = speed > 10 ? speed : 0;
dir = filterDir->calculate(dir);
#ifdef DRIVE_VECTOR_SUM

4
src/test_menu.cpp
View File

@ -114,8 +114,8 @@ void TestMenu::testMenu()
ballPresence->test();
break;
case 'r':
drive->stopAll();
flagtest = false;
DEBUG.println("Roller at default speed");
roller->speed(roller->MAX);
break;
case 's':
DEBUG.println("32u4 send Test");

4
utility/OpenMV/conic_eff_h7.py
View File

@ -46,7 +46,7 @@ blue_led.on()
thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz
(50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
(31, 74, -31, 15, -50, -9)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (50, 0, 250, 200)
@ -67,7 +67,7 @@ clock = time.clock()'''
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.set_windowing(roi)
sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(2)
sensor.set_brightness(3)