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

centering with camera only when both goals seen. Using a pid for each axis and then converging the outputs in an angle. This could be possibly done with multiple movements in a single loop and then merged together in a single one.

code_midgen
EmaMaker 2020-02-26 20:07:14 +01:00
parent 33bdb4462a 3c34a71676
commit 752866de32
6 changed files with 100 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/positionsys_camera.h
View File

@ -1,7 +1,17 @@
#include "PID_v2.h"
#include "systems.h"
#define CAMERA_CENTER_X 3
#define CAMERA_CENTER_Y 6
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y_BOTH 3
#define CAMERA_CENTER_Y_BLUE -60
#define CAMERA_CENTER_Y_YELLOW -30
#define Kpx 1
#define Kix 0
#define Kdx 0
#define Kpy 1
#define Kiy 0
#define Kdy 0
class PositionSysCamera : public PositionSystem{
@ -10,5 +20,12 @@ class PositionSysCamera : public PositionSystem{
void goCenter();
void update() override;
void test() override;
void setCameraPID();
void CameraPID();
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
PID* X;
PID* Y;
};

1
lib/Arduino-PID-Library Submodule

@ -0,0 +1 @@
Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb

2
src/drivecontroller.cpp
View File

@ -50,7 +50,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
vyn = 0;
}
void DriveController::prepareDrive(int dir, int speed, int tilt=0){
void DriveController::prepareDrive(int dir, int speed, int tilt){
pDir = dir;
pSpeed = speed;
pTilt = tilt;

1
src/main.cpp
View File

@ -25,6 +25,7 @@ void loop() {
// Last thing to do: movement and update status vector
// drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix);
drive->drivePrepared();
updateStatusVector();
}

87
src/positionsys_camera.cpp
View File

@ -3,7 +3,9 @@
#include "vars.h"
#include "sensors.h"
PositionSysCamera::PositionSysCamera() {}
PositionSysCamera::PositionSysCamera() {
setCameraPID();
}
void PositionSysCamera::update(){
}
@ -14,26 +16,83 @@ 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);
/* 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);
else drive->prepareDrive(0, 0, 0); */
/*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
// int x = 1;
// int y = 1;
/* int x = 1;
int y = 1;
// //Trying using an angle
// 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);
//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(){
Inputx = 0;
Outputx = 0;
Setpointx = 0;
Inputy = 0;
Outputy = 0;
Setpointy = 0;
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
X->SetOutputLimits(-50,50);
X->SetMode(AUTOMATIC);
X->SetDerivativeLag(1);
X->SetSampleTime(2);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
Y->SetOutputLimits(-50,50);
Y->SetMode(AUTOMATIC);
Y->SetDerivativeLag(1);
Y->SetSampleTime(2);
}
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;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_BOTH;
}
if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
Inputx = CURRENT_DATA_READ.cam_xb;
Inputy = CURRENT_DATA_READ.cam_yb;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_BLUE;
}
if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputx = CURRENT_DATA_READ.cam_xy;
Inputy = CURRENT_DATA_READ.cam_yy;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_YELLOW;
//Setpointy todo
}else{
}
//TODO: no goal seen
X->Compute();
Y->Compute();
// DEBUG.println(Outputx);
int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14);
dir = (dir+360) % 360;
drive->prepareDrive(dir, 100, 0);
}

11
utility/OpenMV/conic_eff.py
View File

@ -38,8 +38,8 @@ blue_led.on()
##############################################################################
thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal
(39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (23, 38, -34, -8, -12, 22), # thresholds yellow goal
(69, 98, -14, 30, 66, 113)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
@ -60,10 +60,11 @@ sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3)
sensor.set_saturation(0)
sensor.set_brightness(-2)
sensor.set_saturation(2)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_exposure(False, 6000)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 5000)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)