69 lines
1.7 KiB
C++
69 lines
1.7 KiB
C++
#pragma once
|
|
|
|
#include "PID_v2.h"
|
|
#include "behaviour_control/complementary_filter.h"
|
|
#include "behaviour_control/status_vector.h"
|
|
#include "systems/systems.h"
|
|
|
|
// Camera center: those setpoints correspond to what we consider the center of the field
|
|
#define CAMERA_CENTER_X 0
|
|
#define CAMERA_CENTER_Y 0
|
|
// left and right limits of a goal
|
|
#define CAMERA_GOAL_MAX_X 8
|
|
#define CAMERA_GOAL_MIN_X (-8)
|
|
|
|
// dimensions of the field, kinda empirical
|
|
#define DIM_X 80
|
|
#define DIM_X_HALF 40
|
|
#define DIM_Y 140
|
|
#define DIM_Y_HALF 70
|
|
|
|
// where is the center of a goal blob as seen by openmv on the field. For atk goal it's positive, for def goal it's negative
|
|
#define CAMERA_GOAL_X 0
|
|
#define CAMERA_GOAL_Y DIM_Y_HALF
|
|
#define CAMERA_GOAL_ATK_Y CAMERA_GOAL_Y
|
|
#define CAMERA_GOAL_DEF_Y (-CAMERA_GOAL_Y)
|
|
|
|
// hipotenuse of dimensions of field
|
|
#define MAX_DIST_EXPERIMENTAL 94
|
|
|
|
#define DIST_MULT 8
|
|
|
|
#define VICINITY_DIST_TRESH 2
|
|
#define ROUGH_VICINITY_DIST_TRESH 10
|
|
|
|
#define Kpx 2.5
|
|
#define Kix 0.1
|
|
#define Kdx 0
|
|
#define Kpy 3.5
|
|
#define Kiy 0.1
|
|
#define Kdy 0
|
|
|
|
class PositionSysCamera : public PositionSystem
|
|
{
|
|
|
|
public:
|
|
PositionSysCamera();
|
|
void goCenter() override;
|
|
void centerGoal() override;
|
|
void setMoveSetpoints(int x, int y);
|
|
void addMoveOnAxis(int x, int y);
|
|
void update() override;
|
|
void test() override;
|
|
void setCameraPID();
|
|
void CameraPID();
|
|
bool isInTheVicinityOf(int, int);
|
|
bool isInRoughVicinityOf(int, int);
|
|
bool isAtDistanceFrom(int, int, int);
|
|
|
|
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
|
|
int MAX_DIST, vx, vy, axisx, axisy, method;
|
|
bool givenMovement;
|
|
PID *X;
|
|
PID *Y;
|
|
ComplementaryFilter *filterDir;
|
|
ComplementaryFilter *filterSpeed;
|
|
|
|
data valid_data;
|
|
};
|