EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1 from EmaMaker/romecup 

Post-Romecup 2021 code
code_newgen
EmaMaker 2021-05-19 17:15:09 +02:00 committed by GitHub
parent e71c49efd1 41fefc63c6
commit 393ee29b7e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
40 changed files with 1112 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/behaviour_control/status_vector.h
View File

@ -22,7 +22,7 @@
#define sv_extr extern
#endif
#define dim 20
#define dim 50
#define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)] )
#define CURRENT_DATA_WRITE ( datas[((currentSVIndex))] )
#define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] )
@ -38,7 +38,7 @@ typedef struct data{
int IMUAngle, ballAngle, ballAngleFix, ballDistance,
yAngle, bAngle, yAngleFix, bAngleFix,
yDist, bDist,
angleAtk, angleAtkFix, angleDef, angleDefFix,
angleAtk, angleAtkFix, angleDef, angleDefFix, 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,
@ -60,4 +60,6 @@ sv_extr data datas[dim];
sv_extr int currentSVIndex;
void initStatusVector();
void updateStatusVector();
void updateStatusVector();
data getDataAtIndex(int index);
data getDataAtIndex_backwardsFromCurrent(int steps);

5
include/motors_movement/drivecontroller.h
View File

@ -20,12 +20,14 @@
//Max possible vel 310
#define MAX_VEL 150
#define MAX_VEL 120
#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)
#define MAX_VEL_QUARTER ((int)MAX_VEL*0.25)
//#define DRIVE_VECTOR_SUM
class DriveController{
public:
@ -38,6 +40,7 @@ class DriveController{
float updatePid();
float torad(float f);
void resetDrive();
void stopAll();
int vxp, vyp, vxn, vyn;
bool canUnlock;

1
include/motors_movement/motor.h
View File

@ -12,6 +12,7 @@ class Motor {
Motor();
void drive(int speed);
void test();
void stop();
public:
int angle;

20
include/motors_movement/roller.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include "ESC.h"
#define ROLLER_DEFAULT_SPEED 1200
class Roller{
public:
Roller(int pinPwm_, int pinSense, int MIN_, int MAX_, int ARM_);
void setup();
void speed(int);
void update();
public:
ESC* roller;
int pinPwm, pinSense, MIN, MAX, ARM;
bool roller_arm_setup, roller_armed;
int roller_setup_phase, roller_counter, roller_speed;
};

9
include/sensors/data_source_ball.h
View File

@ -1,5 +1,11 @@
#pragma once
#include "behaviour_control/data_source.h"
#include <Arduino.h>
#define MOUTH_MIN_ANGLE 340
#define MOUTH_MAX_ANGLE 20
#define MOUTH_DISTANCE 115
#define MOUTH_MAX_DISTANCE 140
class DataSourceBall : public DataSource{
@ -7,6 +13,9 @@ class DataSourceBall : public DataSource{
DataSourceBall(HardwareSerial* ser, int baud);
void postProcess() override;
void test() override;
bool isInMouth();
bool isInMouthMaxDistance();
bool isInFront();
int angle, distance, angleFix;
bool ballSeen;

9
include/sensors/data_source_bt.h
View File

@ -8,8 +8,13 @@ class DataSourceBT : public DataSource{
public:
DataSourceBT(HardwareSerial* ser, int baud);
void test() override;
void update() override;
void connect();
void reconnect();
void receive();
void send();
bool b, comrade;
bool can_bombard, bt_bombarded, comrade;
unsigned long bt_timer, last_received, t;
char received, tosend;
};

14
include/sensors/data_source_camera_conicmirror.h
View File

@ -18,6 +18,18 @@
#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF
/*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
These values need to be subtracted from the coords used in setMoveSetpoints*/
// Robot without roller
// #define CAMERA_TRANSLATION_X 0
// #define CAMERA_TRANSLATION_Y 7
//Robot with roller
#define CAMERA_TRANSLATION_X 0
#define CAMERA_TRANSLATION_Y 7
class DataSourceCameraConic : public DataSource{
public:
@ -35,7 +47,7 @@ class DataSourceCameraConic : public DataSource{
true_xy_fixed, true_yb_fixed, true_yy_fixed;
bool data_received = false, start = false, end = false;
int goalOrientation, pAtk, pDef;
int goalOrientation, old_goalOrientation, pAtk, pDef;
int value;
ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix;

9
include/sensors/sensors.h
View File

@ -10,11 +10,12 @@
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "motors_movement/drivecontroller.h"
#include "motors_movement/motor.h"
#include "motors_movement/roller.h"
#include "motors_movement/drivecontroller.h"
#include "systems/systems.h"
#include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_zone.h"
#include "systems/systems.h"
#include "sensors/data_source_ball.h"
#include "sensors/data_source_bt.h"
#include "sensors/data_source_bno055.h"
@ -36,4 +37,6 @@ s_extr DriveController* drive;
s_extr DataSourceBT* bt;
s_extr int role;
s_extr int robot_indentifier;
s_extr int robot_indentifier;
s_extr Roller* roller;

10
include/strategy_roles/games.h
View File

@ -9,11 +9,13 @@
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "strategy_roles/striker.h"
#include "strategy_roles/striker_test.h"
#include "strategy_roles/keeper.h"
#include "strategy_roles/precision_shooter.h"
#include "strategy_roles/pass_and_shoot.h"
// #include "strategy_roles/keeper.h"
void initGames();
g_extr Game* striker;
g_extr Game* striker_test;
g_extr Game* keeper;
g_extr Game* precision_shooter;
g_extr Game* pass_and_shoot;
// g_extr Game* keeper;

28
include/strategy_roles/pass_and_shoot.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define PAS_ATTACK_DISTANCE 110
#define PAS_TILT_STOP_DISTANCE 140
#define PAS_PLUSANG 55
#define PAS_PLUSANG_VISIONCONE 10
class PassAndShoot : public Game{
public:
PassAndShoot();
PassAndShoot(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
int tilt();
int atk_speed, atk_direction, atk_tilt;
float cstorc;
unsigned long pass_counter;
bool gotta_tilt;
};

30
include/strategy_roles/precision_shooter.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define PS_ATTACK_DISTANCE 110
#define PS_TILT_STOP_DISTANCE 140
#define PS_PLUSANG 55
#define PS_PLUSANG_VISIONCONE 10
class PrecisionShooter : public Game{
public:
PrecisionShooter();
PrecisionShooter(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
int tilt();
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
unsigned long pas_counter;
};

8
include/strategy_roles/striker.h
View File

@ -6,8 +6,8 @@
#define STRIKER_ATTACK_DISTANCE 110
#define STRIKER_TILT_STOP_DISTANCE 140
#define STRIKER_PLUSANG 55
#define STRIKER_PLUSANG_VISIONCONE 10
#define STRIKER_PLUSANG 62
#define STRIKER_PLUSANG_VISIONCONE 7
class Striker : public Game{
@ -25,8 +25,4 @@ class Striker : public Game{
float cstorc;
bool gotta_tilt;
ComplementaryFilter* filter;
};

26
include/strategy_roles/striker_test.h
View File

@ -1,26 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define TARGET_DIST 120
#define ATTACK_DIST 150
#define ANGLE_SHIFT_STEP 5
#define STRIKER_SPD 80
class StrikerTest : public Game{
public:
StrikerTest();
StrikerTest(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
void ballBack();
int atk_speed, atk_direction;
};

19
include/systems/lines/linesys_camera.h
View File

@ -7,18 +7,17 @@
#include "vars.h"
#define S1I A7
#define S1O A6
#define S2I A2
#define S2O A3
#define S3I A1
#define S3O A0
#define S4I A9
#define S4O A8
#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 250
#define LINES_EXIT_SPD 350
#define EXIT_TIME 300
class LineSysCamera : public LineSystem{

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

@ -3,22 +3,18 @@
#include "PID_v2.h"
#include "systems/systems.h"
#include "behaviour_control/complementary_filter.h"
#include "behaviour_control/status_vector.h"
/*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
These values need to be subtracted from the coords used in setMoveSetpoints*/
#define CAMERA_TRANSLATION_X 10
#define CAMERA_TRANSLATION_Y 0
//Camera center: those setpoints correspond to what we consider the center of the field
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0
//Camera goal: those setpoints correspond to the position of the center of the goal on the field
#define CAMERA_GOAL_X 0
#define CAMERA_GOAL_Y -14
#define CAMERA_GOAL_Y -12
#define CAMERA_GOAL_MIN_X -16
#define CAMERA_GOAL_MAX_X 16
#define CAMERA_GOAL_MIN_X -8
#define CAMERA_GOAL_MAX_X 8
#define CAMERA_CENTER_Y_ABS_SUM 60
//Actually it's ± MAX_VAL
@ -55,4 +51,6 @@ class PositionSysCamera : public PositionSystem{
ComplementaryFilter* filterDir;
ComplementaryFilter* filterSpeed;
data valid_data;
};

5
include/vars.h
View File

@ -18,4 +18,9 @@ in the 32U4 code*/
#define SWITCH_DX 38
#define SWITCH_ID 33
#define ROLLER_INA 34
#define ROLLER_INB 35
#define BALL_32U4 Serial2
extr float sins[360], cosins[360];

86
lib/RC_ESC/ESC.cpp Normal file
View File

@ -0,0 +1,86 @@
/*
* Electronic Speed Controller (ESC) - Library
*
*
*/
#include "ESC.h"
// < Constructor >
/*
* Sets the proper pin to output.
*/
ESC::ESC(byte ESC_pin, int outputMin, int outputMax, int armVal)
{
oPin = ESC_pin;
oMin = outputMin;
oMax = outputMax;
oArm = armVal;
}
// < Destructor >
ESC::~ESC()
{
// Nothing to destruct
}
/*
* Calibrate the maximum and minimum PWM signal the ESC is expecting
* depends on the outputMin, outputMax values from the constructor
*/
void ESC::calib(void)
{
myESC.attach(oPin); // attaches the ESC on pin oPin to the ESC object
myESC.writeMicroseconds(oMax);
delay(calibrationDelay);
myESC.writeMicroseconds(oMin);
delay(calibrationDelay);
arm();
}
/*
* Sent a signal to Arm the ESC
* depends on the Arming value from the constructor
*/
void ESC::arm(void)
{
myESC.attach(oPin); // attaches the ESC on pin oPin to the ESC object
myESC.writeMicroseconds (oArm);
}
/*
* Sent a signal to stop the ESC
* depends on the ESC stop pulse value
*/
void ESC::stop(void)
{
myESC.writeMicroseconds (stopPulse);
}
/*
* Sent a signal to set the ESC speed
* depends on the calibration minimum and maximum values
*/
void ESC::speed(int outputESC)
{
oESC = constrain(outputESC, oMin, oMax);
myESC.writeMicroseconds(oESC);
}
/*
* Set the current calibration delay in miliseconds
*
*/
void ESC::setCalibrationDelay(uint32_t calibration_delay)
{
calibrationDelay = calibration_delay;
}
/*
* Set the current Stop pulse in microseconds
*
*/
void ESC::setStopPulse(uint32_t stop_pulse)
{
stopPulse = stop_pulse;
}

47
lib/RC_ESC/ESC.h Normal file
View File

@ -0,0 +1,47 @@
/*
* Electronic Speed Controller (ESC) - Library
*
*
*/
#ifndef ESC_Library
#define ESC_Library
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <Servo.h> // Including the Servo library
class ESC
{
public:
ESC(byte ESC_pin, int outputMin = 1000, int outputMax = 2000, int armVal = 500);
~ESC();
void calib(void);
void arm(void);
void stop(void);
void speed(int ESC_val);
void setCalibrationDelay(uint32_t calibration_delay);
void setStopPulse(uint32_t stop_pulse);
private:
// < Local attributes >
// Hardware
byte oPin; // ESC output Pin
// Calibration
int oMin = 1000;
int oMax = 2000;
int oESC = 1000;
int oArm = 500;
uint32_t calibrationDelay = 8000; // Calibration delay (milisecond)
uint32_t stopPulse = 500; // Stop pulse (microseconds)
Servo myESC; // create servo object to control an ESC
};
#endif
/* Electronic Speed Controller (ESC) - Library */

13
src/behaviour_control/status_vector.cpp
View File

@ -10,8 +10,19 @@ void initStatusVector(){
}
}
data getDataAtIndex(int index){
index = constrain(index, 0, dim-1);
return datas[index];
}
data getDataAtIndex_backwardsFromCurrent(int steps){
steps = constrain(steps, 0, dim-1);
return getDataAtIndex((currentSVIndex-steps+dim) % dim);
}
void updateStatusVector(){
currentSVIndex = (currentSVIndex+1) % dim;
CURRENT_DATA_WRITE = CURRENT_DATA_READ;
CURRENT_INPUT_WRITE = CURRENT_INPUT_READ;
}
}

40
src/main.cpp
View File

@ -7,6 +7,9 @@
#include "strategy_roles/games.h"
#include "vars.h"
#include "test_menu.h"
#include "motors_movement/roller.h"
void updateRoller();
TestMenu* testmenu;
@ -14,6 +17,9 @@ bool striker_condition = false;
bool keeper_condition = false;
void setup() {
pinMode(BUZZER, OUTPUT);
tone(BUZZER, 220, 250);
delay(1500);
DEBUG.begin(115200);
@ -23,33 +29,39 @@ void setup() {
}
testmenu = new TestMenu();
tone(BUZZER, 240, 250);
initStatusVector();
initSensors();
initGames();
delay(500);
delay(250);
drive->prepareDrive(0,0,0);
tone(BUZZER, 260, 250);
initSensors();
delay(500);
tone(BUZZER, 320, 250);
initGames();
delay(250);
//Startup sound
tone(BUZZER, 220.00, 250);
}
tone(BUZZER, 350.00, 250);
drive->prepareDrive(0,0,0);
}
void loop() {
updateSensors();
drive->resetDrive();
striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
keeper_condition = role == LOW;
striker_condition = role == HIGH;
striker->play(1);
striker->play(striker_condition);
keeper->play(keeper_condition);
// if(role) precision_shooter->play(1);
// else pass_and_shoot->play(1);
testmenu->testMenu();
// keeper_condition = role == LOW;
// keeper->play(keeper_condition);
// testmenu->testMenu();
// Last thing to do: movement and update status vector
// // Last thing to do: movement and update status vector
drive->drivePrepared();
updateStatusVector();
}

97
src/motors_movement/drivecontroller.cpp
View File

@ -68,11 +68,13 @@ void DriveController::drive(int dir, int speed, int tilt){
//TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
// Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
// Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
#ifdef DRIVE_VECTOR_SUM
vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
// vx = ((speed * cosins[dir]));
// vy = ((-speed * sins[dir]));
#else
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
#endif
// if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
// vxn = 0;
@ -96,54 +98,54 @@ void DriveController::drive(int dir, int speed, int tilt){
input = delta;
setpoint = tilt;
pid->Compute();
if(pid->Compute()){
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;
speed4 += pidfactor;
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;
speed4 += pidfactor;
// Find the maximum speed and scale all of them for the maximum to be 255
float maxVel = 0;
maxVel = max(abs(speed1), maxVel);
maxVel = max(abs(speed2), maxVel);
maxVel = max(abs(speed3), maxVel);
maxVel = max(abs(speed4), maxVel);
// Find the maximum speed and scale all of them for the maximum to be 255
float maxVel = 0;
maxVel = max(abs(speed1), maxVel);
maxVel = max(abs(speed2), maxVel);
maxVel = max(abs(speed3), maxVel);
maxVel = max(abs(speed4), maxVel);
if(maxVel > 255){
// Ratio to 255
float ratio = maxVel/255;
if(maxVel > 255){
// Ratio to 255
float ratio = maxVel/255;
// //Scale all the velocities
speed1 /= ratio;
speed2 /= ratio;
speed3 /= ratio;
speed4 /= ratio;
// //Scale all the velocities
speed1 /= ratio;
speed2 /= ratio;
speed3 /= ratio;
speed4 /= ratio;
// DEBUG.print(speed1);
// DEBUG.print(" | ");
// DEBUG.print(speed2);
// DEBUG.print(" | ");
// DEBUG.print(speed3);
// DEBUG.print(" | ");
// DEBUG.print(speed4);
// DEBUG.print(" | ");
// DEBUG.println(maxVel);
}
DEBUG.print(speed1);
DEBUG.print(" | ");
DEBUG.print(speed2);
DEBUG.print(" | ");
DEBUG.print(speed3);
DEBUG.print(" | ");
DEBUG.print(speed4);
DEBUG.print(" | ");
DEBUG.println(maxVel);
speed1 = constrain(speed1, -255, 255);
speed2 = constrain(speed2, -255, 255);
speed3 = constrain(speed3, -255, 255);
speed4 = constrain(speed4, -255, 255);
m1->drive((int) speed1);
m2->drive((int) speed2);
m3->drive((int) speed3);
m4->drive((int) speed4);
oldSpeed = speed;
}
speed1 = constrain(speed1, -255, 255);
speed2 = constrain(speed2, -255, 255);
speed3 = constrain(speed3, -255, 255);
speed4 = constrain(speed4, -255, 255);
m1->drive((int) speed1);
m2->drive((int) speed2);
m3->drive((int) speed3);
m4->drive((int) speed4);
oldSpeed = speed;
CURRENT_DATA_WRITE.dir = dir;
CURRENT_DATA_WRITE.speed = speed;
CURRENT_DATA_WRITE.tilt = tilt;
@ -157,4 +159,11 @@ void DriveController::resetDrive(){
CURRENT_DATA_WRITE.addvx = 0;
CURRENT_DATA_WRITE.addvy = 0;
prepareDrive(0,0,0);
}
void DriveController::stopAll(){
m1->stop();
m2->stop();
m3->stop();
m4->stop();
}

4
src/motors_movement/motor.cpp
View File

@ -35,6 +35,10 @@ void Motor::drive(int speed){
analogWrite(pinPwm, speed);
}
void Motor::stop(){
this->drive(0);
}
void Motor::test(){
digitalWriteFast(pinA, 0);
digitalWriteFast(pinB, 1);

97
src/motors_movement/roller.cpp Normal file
View File

@ -0,0 +1,97 @@
#include "motors_movement/roller.h"
#include "vars.h"
/* Our PCB is 3.3V, the ESC uses 5V and has an onboard 5V/3A regulated output
To command the ESC we use a Sparkfun's 3.3V-5V board, which also does 5V-3.3V.
To PWM command the ESC we pass a PWM pin to the adapter board.
We use the same adapter board to adapt the ESC's internally regulated 5V to 3.3V.
Since the ESC is turned on (and outputs) only when the motors are turned on, we can detect when the ESC is turned on and needs the startup sequence*/
unsigned long t = 0;
unsigned long t1 = 0;
Roller::Roller(int pinPwm_, int pinSense_, int MIN_, int MAX_, int ARM_){
pinPwm = pinPwm_;
pinSense = pinSense_;
MIN = MIN_;
MAX = MAX_;
ARM = ARM_;
roller_armed = false;
roller_arm_setup = false;
roller_setup_phase = 99;
roller_counter = 0;
roller_speed = 0;
pinMode(pinSense, INPUT_PULLUP);
roller = new ESC(pinPwm, MIN, MAX, ARM);
}
void Roller::setup(){
if(roller_setup_phase == 0){
roller->arm();
roller_setup_phase=1;
roller_counter = 0;
roller_armed = false;
}else if(roller_setup_phase == 1){
if(millis() - t1 > 1){
t1 = millis();
roller->speed(roller_counter);
roller_counter++;
if(roller_counter > 255) roller_setup_phase = 2;
}
}else if(roller_setup_phase == 2){
if(millis() - t1 > 1){
t1 = millis();
roller->speed(roller_counter);
roller_counter--;
if(roller_counter <= 0) roller_setup_phase = 3;
}
}else if(roller_setup_phase == 3){
roller_setup_phase = 4;
roller_armed = true;
}
// DEBUG.print(roller_setup_phase);
// DEBUG.print(" --- ");
// DEBUG.println(roller_counter);
// roller->arm();
// delay(1000);
// for(int i = MIN; i < MAX; i++){
// roller->speed(i);
// delay(1);
// }
// for(int i = MAX; i > MIN; i--){
// roller->speed(i);
// delay(1);
// }
}
void Roller::update(){
bool arm = digitalReadFast(pinSense);
if(arm){
if(!roller_arm_setup){
roller_setup_phase = 0;
roller_arm_setup = true;
roller_armed = false;
}
}else{
roller_armed = false;
roller_arm_setup = false;
}
this->setup();
if(millis() - t > 10 && roller_armed){
roller->speed(roller_speed);
t = millis();
}
}
void Roller::speed(int speed_){
if(roller_armed)
roller_speed = speed_;
}

12
src/sensors/data_source_ball.cpp
View File

@ -38,4 +38,16 @@ void DataSourceBall :: test(){
// }else{
// DEBUG.println("Not seeing ball");
// }
}
bool DataSourceBall::isInFront(){
return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE );
}
bool DataSourceBall::isInMouth(){
return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE);
}
bool DataSourceBall::isInMouthMaxDistance(){
return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE);
}

86
src/sensors/data_source_bt.cpp
View File

@ -1,33 +1,79 @@
#include "sensors/data_source_bt.h"
//if needed, substitute Serial1 with Serial3 to return to the old code setup
/*Currently using a master-slave setup
One bt has been setup using
SM,3 - Master mode
SA, 0 - Disable authentication
SR, xxxx - Address of the other bt
SU,96 - baud rate 9600
SN, name - Name for clarity
The other one
SM,0 - Master mode
SA, 0 - Disable authentication
SU,96 - baud rate 9600
SN, name - Name for clarity
ST, 5 - Configuration timeout
For now it seems that a slave can easily recovery from a master restart, but not the opposite. We will make sure that doesn't happen
*/
DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
// connect();
bt_timer = millis();
can_bombard = false;
bt_bombarded = false;
comrade = false;
last_received = 0;
t = 0;
tosend = 'A';
received = '0';
connect();
}
void DataSourceBT :: connect(){
Serial1.print("$");
Serial1.print("$");
Serial1.print("$");
delay(100);
Serial1.println("C");
//Give the bt time to be brought up (about 5-6 secs.)
//When turned on the bt onboard led will blink. After 5-6 secs it will start blinking at a lower freq. We need to wait for that to happen
// if(millis() >= bt_timer + 5000 && !bt_bombarded){
// can_bombard = true;
// }
// //For a sec, bombard the other bt of packets. Since we are in sm2 this will make them connect. Note that the two of them cannot accept connections while they're trying to connect
// if(can_bombard && millis() >= bt_timer + 5000 && millis() <= bt_timer + 6000){
// DEBUG.println("Bombarding");
// Serial1.print(tosend);
// bt_bombarded = true;
// }else{
// can_bombard = false;
// }
}
void DataSourceBT :: reconnect(){
if(!comrade){
if(!b){
Serial1.print("$");
Serial1.print("$");
Serial1.print("$");
}else{
Serial1.println("C");
}
b = !b;
}else{
Serial1.println("---");
void DataSourceBT :: receive(){
while(Serial1.available()) {
last_received = millis();
received = (char) Serial1.read();
comrade = true;
}
if(millis() - last_received > 2000)
comrade = false;
}
void DataSourceBT::send(){
if(millis() - t >= 250){
Serial1.print(tosend);
}
}
void DataSourceBT::update(){
// if(!bt_bombarded && can_bombard) connect();
receive();
send();
// if(comrade)Serial2.write(0b00000100);
}
void DataSourceBT :: test(){

45
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -1,5 +1,6 @@
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_camera_conicmirror.h"
#include "vars.h"
//Comment out to disable complementary filters on angles
#define CAMERA_CONIC_FILTER_POINTS
@ -31,6 +32,9 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
true_xy_fixed = 0;
true_yy_fixed = 0;
goalOrientation = 0;
old_goalOrientation = 0;
filter_yy = new ComplementaryFilter(FILTER_YY_COEFF);
filter_xy = new ComplementaryFilter(FILTER_YX_COEFF);
filter_yb = new ComplementaryFilter(FILTER_BY_COEFF);
@ -83,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;
true_yb = true_yb - 50;
true_xy = 50 - true_xy;
true_yy = true_yy - 50;
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;
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb = filter_xb->calculate(true_xb);
@ -120,9 +124,6 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_xy_fixed = (true_xy*(cos(angleFix))) - (true_yy*(sin(angleFix)));
true_yy_fixed = (true_xy*(sin(angleFix))) + (true_yy*(cos(angleFix)));
yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed);
true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed);
@ -130,6 +131,9 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed);
#endif
yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.cam_xb = true_xb;
@ -156,17 +160,44 @@ 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.yAtk = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed;
CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xy;
CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xy_fixed;
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.yDef = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed;
CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xb;
CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xb_fixed;
} else {
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.yAtk = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed;
CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xb;
CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xb_fixed;
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.yDef = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed;
CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xy;
CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xy_fixed;
}
byte to_32u4 = 0;
to_32u4 |= (CURRENT_DATA_READ.ySeen);
to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
BALL_32U4.write(to_32u4);
}
void DataSourceCameraConic::test(){

22
src/sensors/sensors.cpp
View File

@ -7,22 +7,32 @@ void initSensors(){
pinMode(SWITCH_ID, INPUT);
drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
//drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
// tone(BUZZER, 270, 250);
// delay(350);
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial2, 57600);
//ball = new DataSourceBall(&Serial4, 57600);
// tone(BUZZER, 275, 250);
// delay(350);
ball = new DataSourceBall(&BALL_32U4, 57600);
// tone(BUZZER, 280, 250);
// delay(350);
camera = new DataSourceCameraConic(&Serial3, 19200);
//camera = new DataSourceCameraConic(&Serial2, 19200);
bt = new DataSourceBT(&Serial1, 115200);
//bt = new DataSourceBT(&Serial3, 115200);
// tone(BUZZER, 285, 250);
// delay(350);
bt = new DataSourceBT(&Serial1, 9600);
roller = new Roller(30, 31, 1000, 2000, 500);
}
void updateSensors(){
role = digitalRead(SWITCH_DX);
camera->old_goalOrientation = camera ->goalOrientation;
camera->goalOrientation = digitalRead(SWITCH_SX);
robot_indentifier = digitalRead(SWITCH_ID);
compass->update();
ball->update();
camera->update();
bt->update();
roller->update();
}

4
src/strategy_roles/games.cpp
View File

@ -13,5 +13,7 @@ void initGames(){
// striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera());
precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
// keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

1
src/strategy_roles/keeper.cpp
View File

@ -16,6 +16,7 @@ Keeper::Keeper() : Game()
Keeper::Keeper(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void Keeper::init()

103
src/strategy_roles/pass_and_shoot.cpp Normal file
View File

@ -0,0 +1,103 @@
#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/pass_and_shoot.h"
#include "vars.h"
#include "math.h"
PassAndShoot::PassAndShoot() : Game()
{
init();
}
PassAndShoot::PassAndShoot(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void PassAndShoot::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
pass_counter = millis();
}
unsigned long pass_timer = 0;
boolean flag = false;
void PassAndShoot::realPlay()
{
if (CURRENT_DATA_READ.ballSeen){
if(robot_indentifier == HIGH){
if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90){
if(!flag){
pass_timer = millis();
flag = true;
//Show on 32u4
Serial2.write(0b00000100);
}
}
if(!flag || (millis() - pass_timer <= 650)){
striker();
}else{
((PositionSysCamera*)ps)->setMoveSetpoints(CAMERA_GOAL_MIN_X, CAMERA_GOAL_Y);
bt->tosend = 'C';
roller->speed(0);
}
}else{
if(bt->received == 'C'){
striker();
}else drive->prepareDrive(0,0,0);
}
}else drive->prepareDrive(0,0,0);
}
void PassAndShoot::striker(){
#ifdef MAX_VEL
#undef MAX_VEL
#define MAX_VEL 100
#endif
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PAS_PLUSANG;
if(ball_deg >= 346 || ball_deg <= 16) plusang = PAS_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
}
int PassAndShoot::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;
}

98
src/strategy_roles/precision_shooter.cpp Normal file
View File

@ -0,0 +1,98 @@
#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/precision_shooter.h"
#include "vars.h"
#include "math.h"
PrecisionShooter::PrecisionShooter() : Game()
{
init();
}
PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void PrecisionShooter::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
}
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->striker();
else
ps->goCenter();
}
unsigned long t3 = 0;
unsigned long t4 = 0;
boolean ignited = false;
void PrecisionShooter::striker(){
#ifdef MAX_VEL
#undef MAX_VEL
#define MAX_VEL 100
#endif
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PS_PLUSANG;
if(ball_deg >= 346 || ball_deg <= 16) plusang = PS_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
// if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
// else roller->speed(roller->MIN);
if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 78 && ( (CURRENT_DATA_READ.posy >= 32 && (CURRENT_DATA_READ.posx >= 15 || CURRENT_DATA_READ.posx <= -15)) || abs(tilt()) > 65 ) ) {
// Just let the robot slowly approach the ball
if(!ignited){
ignited = true;
t4 = millis();
}
if(millis() - t4 > 250 && ignited){
t3 = millis();
}
}
if(millis() - t3 < 800){
// roller->speed(roller->MAX);
drive->prepareDrive(180, MAX_VEL_3QUARTERS, 0);
ignited = false;
}
}
int PrecisionShooter::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;
}

66
src/strategy_roles/striker.cpp
View File

@ -1,11 +1,12 @@
#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.h"
#include "vars.h"
#include "math.h"
Striker::Striker() : Game()
{
init();
@ -24,8 +25,6 @@ void Striker::init()
cstorc = 0;
gotta_tilt = false;
filter = new ComplementaryFilter(0.7);
}
void Striker::realPlay()
@ -36,48 +35,35 @@ void Striker::realPlay()
ps->goCenter();
}
void Striker::striker()
{
int plusang = STRIKER_PLUSANG, ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle;
void Striker::striker(){
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE)
{
drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_HALF, 0);
return;
}
if (ball_deg > 340 || ball_deg < 20)
plusang -= STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if (ball_deg > 180)
ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else
ball_degrees2 = ball_deg;
if (ball_degrees2 > 0)
dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else
dir = ball_deg - plusang; //se sto nel negativo sottraggo
if (dir < 0)
dir = dir + 360; //se sto nel quadrante negativo ricappotto
else
dir = dir;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
}
int Striker::tilt()
{
if (CURRENT_DATA_READ.ballDistance <= STRIKER_ATTACK_DISTANCE) gotta_tilt = true;
if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE && gotta_tilt) gotta_tilt = false;
if (CURRENT_DATA_READ.atkSeen && gotta_tilt) return 0;
if (CURRENT_DATA_READ.ballAngle >= 345 || CURRENT_DATA_READ.ballAngle <= 15) atk_tilt = CURRENT_DATA_READ.angleAtkFix;
int Striker::tilt() {
if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
// if (CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10)
// atk_tilt = (constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45) + 360) % 360;
// else if((CURRENT_DATA_READ.ballAngle > 345 && CURRENT_DATA_READ.ballAngle < 350) || (CURRENT_DATA_READ.ballAngle > 10 && CURRENT_DATA_READ.ballAngle < 15))
// atk_tilt = 0;
atk_tilt = filter->calculate(atk_tilt);
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;
}

100
src/strategy_roles/striker_test.cpp
View File

@ -1,100 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "sensors/sensors.h"
#include "strategy_roles/striker_test.h"
#include "vars.h"
#include "math.h"
StrikerTest::StrikerTest() : Game() {
init();
}
StrikerTest::StrikerTest(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
init();
}
void StrikerTest::init(){
atk_speed = 0;
atk_direction = 0;
}
void StrikerTest::realPlay(){
if(CURRENT_DATA_READ.ballSeen) striker();
else ps->goCenter();
}
void StrikerTest::striker()
{
/*First implementation of "orbital striker", a new way to approach the problem with less lines.
It works with robot's positions, calculating the final one using the angle shift over
and over again until we have an acceptable result. That result is used to drive->prepareDrive with the speed.*/
// if (CURRENT_DATA_READ.ballDistance > )
// drive->prepareDrive(CURRENT_DATA_READ.ballAngle, STRIKER_SPD);
// else
// {
// if (CURRENT_DATA_READ.ballAngle > 340 && CURRENT_DATA_READ.ballAngle < 20)
// {
// drive->prepareDrive(0, 100, 0);
// }
// else
// {
// int ballAngle = (90 - CURRENT_DATA_READ.ballAngle + 360) % 360;
// robotAngle = (ballAngle - 180 + 360) % 360;
// DEBUG.print("Ball is at angle (goniometric circle): ");
// DEBUG.println(ballAngle);
// DEBUG.print("Robot is at angle ");
// DEBUG.println(robotAngle);
// float robotAngle_rad = robotAngle*3.14 / 180;
// robotX = CURRENT_DATA_READ.ballDistance * cos(robotAngle_rad);
// robotY = CURRENT_DATA_READ.ballDistance * sin(robotAngle_rad);
// DEBUG.print("Coords of the robot relative to the ball: (");
// DEBUG.print(robotX);
// DEBUG.print(", ");
// DEBUG.print(robotY);
// DEBUG.println(")");
// // angleDiff = min(((-robotAngle + 360) % 360), ((robotAngle + 360) % 360));
// // angleShift = min(angleDiff, ANGLE_SHIFT_STEP);
// angleShift = ANGLE_SHIFT_STEP;
// if (robotAngle >= 0 && robotAngle <= 180)
// newAngle = robotAngle - angleShift;
// else
// newAngle = robotAngle + angleShift;
// DEBUG.print("New ball-robot angle: ");
// DEBUG.println(newAngle);
// float newAngle_rad = (newAngle)*3.14 / 180;
// robotX_new = TARGET_DIST * cos(newAngle_rad);
// robotY_new = TARGET_DIST * sin(newAngle_rad);
// DEBUG.print("New coords of the robot relative to the ball: (");
// DEBUG.print(robotX_new);
// DEBUG.print(", ");
// DEBUG.print(robotY_new);
// DEBUG.println(")");
// moveAngle = (atan2((robotX_new - robotX), (robotY_new - robotY))) * 180 / 3.14;
// moveAngle = (moveAngle + 360) % 360;
// DEBUG.print("Direction to move in: ");
// DEBUG.println(moveAngle);
// drive->prepareDrive(moveAngle, STRIKER_SPD);
// }
// }
// delay(1000);
// DEBUG.println("==========");
}
void StrikerTest::ballBack() {
}

2
src/system/lines/linesys_camera.cpp
View File

@ -78,7 +78,7 @@ void LineSysCamera::outOfBounds(){
}
if (millis() - exitTimer < EXIT_TIME){
CURRENT_DATA_WRITE.game->ps->goCenter();
CURRENT_DATA_READ.game->ps->goCenter();
tookLine = true;
tone(BUZZER, 220.00, 250);
}else{

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

@ -30,7 +30,7 @@ PositionSysCamera::PositionSysCamera() {
Y->SetDerivativeLag(1);
Y->SetSampleTime(2);
filterDir = new ComplementaryFilter(0.65);
filterDir = new ComplementaryFilter(0.35);
filterSpeed = new ComplementaryFilter(0.65);
}
@ -48,12 +48,31 @@ void PositionSysCamera::update(){
}else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){
posx = CURRENT_DATA_WRITE.cam_xy;
posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
}else{
//TODO: no goal seen ?
}
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
//x = 66 is a very very strange bug I can't seem to track down. It's a dirty hack, I know
if(posx == 66 || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
// Go back in time until we found a valid status, when we saw at least one goal
int i = 1;
do{
valid_data = getDataAtIndex_backwardsFromCurrent(i);
i++;
}while(!valid_data.ySeen && !valid_data.bSeen);
if(valid_data.ySeen || valid_data.bSeen){
posx = valid_data.posx;
posy = valid_data.posy;
// Trick the status vector into thinking this was a valid status
CURRENT_DATA_WRITE.ySeen = valid_data.ySeen;
CURRENT_DATA_WRITE.bSeen = valid_data.bSeen;
}
}
CURRENT_DATA_WRITE.posx = posx;
CURRENT_DATA_WRITE.posy = posy;
@ -71,8 +90,10 @@ void PositionSysCamera::update(){
//This means the last time this is called has the biggest priority, has for prepareDrive
void PositionSysCamera::setMoveSetpoints(int x, int y){
Setpointx = x + CAMERA_TRANSLATION_X;
Setpointy = y + CAMERA_TRANSLATION_Y;
// Setpointx = x + CAMERA_TRANSLATION_X;
// Setpointy = y + CAMERA_TRANSLATION_Y;
Setpointx = x;
Setpointy = y;
givenMovement = true;
CameraPID();
}
@ -123,21 +144,19 @@ void PositionSysCamera::CameraPID(){
int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
// int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
speed = filterSpeed->calculate(speed);
speed = speed > 40 ? speed : 0;
dir = filterDir->calculate(dir);
// drive->prepareDrive(dir, speed, 0);
speed = speed > 30 ? speed : 0;
dir = filterDir->calculate(dir);;
//speed = filterSpeed->calculate(speed);
//Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
//and check the notes in drivecontroller for the other stuff to comment and uncomment
//TODO: add complementary filter on this speed if we keep using it
#ifdef DRIVE_VECTOR_SUM
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy;
#else
drive->prepareDrive(dir, speed, 0);
#endif
}
}

4
src/system/positions/positionsys_empty.cpp
View File

@ -15,9 +15,9 @@ void PositionSystemEmpty::test(){
}
void PositionSystemEmpty::goCenter(){
drive->prepareDrive(0,0,0);
// drive->prepareDrive(0,0,0);
}
void PositionSystemEmpty::centerGoal(){
drive->prepareDrive(0,0,0);
// drive->prepareDrive(0,0,0);
}

7
src/test_menu.cpp
View File

@ -55,10 +55,12 @@ void TestMenu::testMenu()
break;
case '3':
DEBUG.println("Motors Test. Lift up from ground and turn on S_MOT");
drive->stopAll();
drive->m1->test();
drive->m2->test();
drive->m3->test();
drive->m4->test();
flagtest = true;
break;
case '4':
DEBUG.println("Pid recenter test");
@ -126,6 +128,10 @@ void TestMenu::testMenu()
Serial2.write(0);
delay(1500);
break;
case 'r':
drive->stopAll();
flagtest = false;
break;
case 'h':
default:
DEBUG.println("Unknown command, here's a lil help for you :)");
@ -143,6 +149,7 @@ void TestMenu::testMenu()
DEBUG.println("9)Switches test");
DEBUG.println("u)Read Serial messages from 32u4");
DEBUG.println("s)Send test to 32u4 status LEDs");
DEBUG.println("r)Test roller");
flagtest = false;
}
}

20
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (54, 73, -4, 17, 18, 73), # thresholds yellow goal
(36, 45, -7, 7, -35, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
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)
roi = (80, 0, 240, 230)
roi = (50, 0, 250, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -67,13 +67,13 @@ 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(0)
sensor.set_brightness(1)
sensor.set_auto_whitebal(True)
sensor.set_saturation(2)
sensor.set_brightness(3)
sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -1.804))
sensor.set_auto_exposure(False, 6576)
sensor.set_auto_gain(True)
#sensor.set_auto_gain(False, gain_db=8.78)
sensor.skip_frames(time = 300)
clock = time.clock()
@ -83,7 +83,7 @@ clock = time.clock()
while(True):
clock.tick()
print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + "White Bal: " + str(sensor.get_rgb_gain_db()))
print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db()))
blue_led.off()
@ -94,7 +94,7 @@ while(True):
tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata
img = sensor.snapshot()
for blob in img.find_blobs(thresholds, pixels_threshold=50, area_threshold=70, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())

219
utility/OpenMV/conic_eff_h7.py.autosave Normal file
View File

@ -0,0 +1,219 @@
# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
# Based on:
# color tracking - By: paolix - ven mag 18 2018
# Automatic RGB565 Color Tracking Example
#
import sensor, image, time, pyb, math
from pyb import UART
uart = UART(3,19200, timeout_char = 1000)
START_BYTE = chr(105) #'i'
END_BYTE = chr(115) #'s'
BYTE_UNKNOWN = chr(116) #'t'
y_found = False
b_found = False
#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
def val_map(x, in_min, in_max, out_min, out_max):
x = int(x)
in_min = int(in_min)
in_max = int(in_max)
out_min = int(out_min)
out_max = int(out_max)
return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
# Check side
def isInLeftSide(img, x):
return x < img.width() / 2
def isInRightSide(img, x):
return x > img.width() / 2
# LED Setup ##################################################################
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
red_led.off()
green_led.off()
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)
roi = (50, 0, 250, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.set_auto_exposure(False, 10000) vbc
#sensor.set_backlight(1)
#sensor.set_brightness(+2 )
#sensor.set_windowing(roi)
clock = time.clock()'''
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(2)
sensor.set_brightness(3)
sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -1.804))
sensor.set_auto_exposure(False, 6576)
#sensor.set_auto_gain(False, gain_db=8.78)
sensor.skip_frames(time = 300)
clock = time.clock()
##############################################################################
while(True):
clock.tick()
print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db()))
blue_led.off()
y_found = False
b_found = False
tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata
img = sensor.snapshot()
for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())
if (blob.code() == 1):
tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
y_found = True
if (blob.code() == 2):
tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
b_found = True
tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste
tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste
ny = len(tt_yellow)
nb = len(tt_blue)
#Formulas to compute position of points, considering that the H7 is rotated by a certain angle
#x = y-offset
#y = offset - x
#Compute everything related to Yellow First
y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
y_cx = int(y1_cy - img.height() / 2)
y_cy = int(img.width() / 2 - y1_cx)
#Normalize data between 0 and 100
if y_found == True:
img.draw_cross(y1_cx, y1_cy)
y_cx = val_map(y_cx, -img.height() / 2, img.height() / 2, 100, 0)
y_cy = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 100)
#Prepare for send as a list of characters
s_ycx = chr(y_cx)
s_ycy = chr(y_cy)
else:
y_cx = BYTE_UNKNOWN
y_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_ycx = y_cx
s_ycy = y_cy
#Compute everything relative to Blue
'''Given the light situation in our lab and given that blue is usually harder to spot than yellow, we need to check it we got
a blue blob that is in the same side of the ground as the yellow one, if so, discard it and check a new one
'''
b_cx = BYTE_UNKNOWN
b_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_bcx = b_cx
s_bcy = b_cy
if b_found == True:
for i in range(nb-1, 0,-1):
b_area, b1_cx, b1_cy, b_code = tt_blue[i]
if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
img.draw_cross(b1_cx, b1_cy)
b_cx = int(b1_cy - img.height() / 2)
b_cy = int(img.width() / 2 - b1_cx)
#print("before :" + str(b_cx) + " " + str(b_cy))
b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
#print("after :" + str(b_cx) + " " + str(b_cy))
#Prepare for send as a list of characters
s_bcx = chr(b_cx)
s_bcy = chr(b_cy)
'''index = 1
if b_found == True:
while nb-index >= 0:
b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index]
index += 1
# If the two blobs are on opposide side of the field, everything is good
if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
img.draw_cross(b1_cx, b1_cy)
b_cx = int(b1_cy - img.height() / 2)
b_cy = int(img.width() / 2 - b1_cx)
print("before :" + str(b_cx) + " " + str(b_cy))
b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
print("after :" + str(b_cx) + " " + str(b_cy))
#Prepare for send as a list of characters
s_bcx = chr(b_cx)
s_bcy = chr(b_cy)
break
else:
b_cx = BYTE_UNKNOWN
b_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_bcx = b_cx
s_bcy = b_cy'''
#print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy))
uart.write(START_BYTE)
uart.write(s_bcx)
uart.write(s_bcy)
uart.write(s_ycx)
uart.write(s_ycy)
uart.write(END_BYTE)

BIN
utility/bluetooth/rn42_test_sm2/rn42_test_sm2.ino
View File

Binary file not shown.