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

position system camera and vector sum system to do multiple movements in a single one. obv to be tested, but it's difficult without robot and field

pull/1/head
EmaMaker 2020-03-12 12:09:13 +01:00
parent 4cd4b8f9d6
commit f62d013675
9 changed files with 122 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/behaviour_control/status_vector.h
View File

@ -43,7 +43,8 @@ typedef struct data{
speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr,
lineOutDir, matePos, role, cam_xb_fixed,
cam_xy_fixed, cam_yb_fixed, cam_yy_fixed;
cam_xy_fixed, cam_yb_fixed, cam_yy_fixed,
posx, posy, addvx, addvy;
Game* game;
LineSystem* lineSystem;
PositionSystem* posSystem;

2
include/motors_movement/drivecontroller.h
View File

@ -51,6 +51,4 @@ class DriveController{
double input, output, setpoint;
float sins[360], cosins[360];
};

6
include/position/positionsys_camera.h
View File

@ -24,6 +24,9 @@ class PositionSysCamera : public PositionSystem{
public:
PositionSysCamera();
void goCenter();
void centerGoal();
void setMoveSetpoints(int x, int y);
void addMoveOnAxis(int x, int y);
void update() override;
void test() override;
void setCameraPID();
@ -31,7 +34,8 @@ class PositionSysCamera : public PositionSystem{
int calcOtherGoalY(int goalY);
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
int MAX_DIST;
int MAX_DIST, vx, vy, axisx, axisy;
bool givenMovement;
PID* X;
PID* Y;

11
include/vars.h
View File

@ -1,6 +1,12 @@
#pragma once
#define DEBUG Serial3
#ifdef VARS
#define extr
#else
#define extr extern
#endif
#define GLOBAL_SPD_MULT 1.0
#define LED_R 20
@ -9,4 +15,7 @@
#define BUZZER 30
#define SWITCH_SX 28
#define SWITCH_DX 29
#define SWITCH_DX 29
extr float sins[360], cosins[360];

10
src/main.cpp
View File

@ -1,14 +1,22 @@
#include <Arduino.h>
#define VARS
#include "behaviour_control/status_vector.h"
#include "position/positionsys_zone.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
#include "vars.h"
void setup() {
delay(1500);
DEBUG.begin(9600);
for(int i = 0; i < 360; i++){
sins[i] = (float) sin((i*3.14/180));
cosins[i] = (float) cos((i*3.14/180));
}
initStatusVector();
initSensors();
initGames();

11
src/motors_movement/drivecontroller.cpp
View File

@ -1,17 +1,13 @@
#include "motors_movement/drivecontroller.h"
#include "sensors/sensors.h"
#include "behaviour_control/status_vector.h"
#include "vars.h"
DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
m1 = m1_;
m2 = m2_;
m3 = m3_;
m4 = m4_;
for(int i = 0; i < 360; i++){
sins[i] = (float) sin(torad(i));
cosins[i] = (float) cos(torad(i));
}
pDir = 0;
pSpeed = 0;
@ -69,8 +65,9 @@ void DriveController::drive(int dir, int speed, int tilt){
speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT;
tilt = tilt > 180 ? tilt - 360 : tilt;
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
//TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
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;

162
src/position/positionsys_camera.cpp
View File

@ -4,46 +4,6 @@
#include "vars.h"
PositionSysCamera::PositionSysCamera() {
setCameraPID();
}
void PositionSysCamera::update(){
}
void PositionSysCamera::test(){
}
void PositionSysCamera::goCenter(){
/*WORKS BUT CAN BE BETTER*/
//Y
/* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
//X
else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
else drive->prepareDrive(0, 0, 0); */
/*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
/* int x = 1;
int y = 1;
;
//Trying using an angle
if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y)
y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
int dir = -90-(atan2(y*1.5,x)*180/3.14);
dir = (dir+360) % 360;
drive->prepareDrive(dir, 100, 0);
} */
CameraPID();
}
//using a pid controller for the movement, or trying at least
void PositionSysCamera :: setCameraPID(){
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
Inputx = 0;
Outputx = 0;
@ -51,6 +11,12 @@ void PositionSysCamera :: setCameraPID(){
Inputy = 0;
Outputy = 0;
Setpointy = 0;
vx = 0;
vy = 0;
axisx = 0;
axisy = 0;
givenMovement = false;
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
X->SetOutputLimits(-50,50);
@ -64,9 +30,65 @@ void PositionSysCamera :: setCameraPID(){
Y->SetSampleTime(2);
}
void PositionSysCamera::update(){
int posx, posy;
//Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync
//Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot
if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){
posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2;
posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy;
}else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){
posx = CURRENT_DATA_WRITE.cam_xb;
posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb);
}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;
CURRENT_DATA_WRITE.posx = posx;
CURRENT_DATA_WRITE.posy = posy;
Inputx = posx;
Inputy = posy;
//Prepare for receiving information about movement
//Starting setpoint position as current position
Setpointx = posx;
Setpointy = posy;
axisx = 0;
axisy = 0;
givenMovement = false;
}
//This means the last time this is called has the biggest priority, has for prepareDrive
void PositionSysCamera::setMoveSetpoints(int x, int y){
Setpointx = x;
Setpointy = y;
givenMovement = true;
}
void PositionSysCamera::addMoveOnAxis(int x, int y){
axisx += x;
axisy += y;
givenMovement = true;
}
void PositionSysCamera::goCenter(){
setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
}
void PositionSysCamera::centerGoal(){
setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y);
}
/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one
We know the sum of the absolute values is a fixed number.
By♦ subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
The sign of the goal y we found is simply the reverse of the one we got
*/
int PositionSysCamera::calcOtherGoalY(int goalY){
@ -75,46 +97,36 @@ int PositionSysCamera::calcOtherGoalY(int goalY){
return otherGoalY;
}
void PositionSysCamera :: CameraPID(){
if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2;
Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
}else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
Inputx = CURRENT_DATA_READ.cam_xb;
Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb);
}else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputx = CURRENT_DATA_READ.cam_xy;
Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
//Setpointy todo
}else{
//TODO: no goal seen
}
void PositionSysCamera::CameraPID(){
if(givenMovement){
//To have the axis corresponding to the real cartesian plane Inputx and InputY must be multiplied by -1
//This is because the coordinates given as input are relative to the robot, while the coordinates used in the setpoint
//are on an absolute cartesian plane with it's center in the center of the field
Inputx *= -1;
Inputy *= -1;
vx = 0;
vy = 0;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y;
Setpointx += axisx;
Setpointy += axisy;
X->Compute();
Y->Compute();
X->Compute();
Y->Compute();
// DEBUG.print(CURRENT_DATA_READ.cam_yb);
// DEBUG.print(" ");
// DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
/*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
drive->prepareDrive(0,0,0);
}else{*/
//Compute an X and Y to give to the PID later
//There's surely a better way to do this
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
drive->prepareDrive(dir, speed, 0);
//DEBUG.println(dir);
//}
}
//TODO: add complementary filter on this speed if we keep using it
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy;
}
}
void PositionSysCamera::test(){
}

2
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -45,6 +45,8 @@ void DataSourceCameraConic ::readSensor() {
true_yb = yb;
true_xy = xy;
true_yy = yy;
computeCoordsAngles();
}
end = true;
start = false;

5
src/strategy_roles/game.cpp
View File

@ -1,5 +1,6 @@
#include "behaviour_control/status_vector.h"
#include "strategy_roles/game.h"
#include "position/positionsys_camera.h"
Game::Game() {}
Game::Game(LineSystem* ls_, PositionSystem* ps_) {
@ -8,14 +9,14 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) {
}
void Game::play(bool condition){
ps->update();
if(condition) {
ps->update();
realPlay();
ls->update();
CURRENT_DATA_WRITE.posSystem = (this->ps);
CURRENT_DATA_WRITE.lineSystem = (this->ls);
CURRENT_DATA_WRITE.game = this;
((PositionSysCamera*)ps)->CameraPID();
}
}