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

best pid YET

pull/1/head
EmaMaker 2020-02-19 17:44:31 +01:00
parent f2903c2c6b
commit 2ac684220f
11 changed files with 45 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/drivecontroller.h
View File

@ -6,8 +6,8 @@
//PID Constants
#define KP 1.5
#define KI 0.0
#define KD 0.3
#define KI 0
#define KD 0.1
#define UNLOCK_THRESH 800

2
include/sensors.h
View File

@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
s_extr DataSourceBNO055* compass;
s_extr DataSourceBall* ball;
s_extr DataSourceCameraConic* camera;
s_extr DataSourceCameraVShaped* camera;
s_extr DriveController* drive;
s_extr DataSourceBT* bt;

6
lib/ArduinoPIDLibraryFlavio/PID_v2.cpp
View File

@ -65,6 +65,12 @@ bool PID::Compute()
/*Compute all the working error variables*/
double input = *myInput;
double error = *mySetpoint - input;
if(angleWrap){
if(error < -179) error += 360;
if(error > 180) error -= 360;
}
double dInput = (input - lastInput);
outputSum+= (ki * error);

6
lib/ArduinoPIDLibraryFlavio/PID_v2.h
View File

@ -59,6 +59,10 @@ class PID
kd_lagpam = val;
}
void setAngleWrap(bool a){
angleWrap = a;
}
double getDerivative(){
return filteredDerivative;
}
@ -98,6 +102,6 @@ class PID
unsigned long SampleTime;
double outMin, outMax;
bool inAuto, pOnE;
bool inAuto, pOnE, angleWrap;
};
#endif

26
src/data_source_camera_vshapedmirror.cpp
View File

@ -88,22 +88,24 @@ void DataSourceCameraVShaped :: postProcess(){
pDef = valY * -1;
}
//attacco gialla
if(goalOrientation == HIGH){
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleAtk = valY;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleDefFix = valB;
}else{
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleAtkFix = valB;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleDefFix = valY;
}
datavalid = 0;
cameraReady = 1; //attivo flag di ricezione pacchetto
}
//attacco gialla
if(goalOrientation == HIGH){
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleAtk = valY;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleDefFix = valB;
}else{
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleAtkFix = valB;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleDefFix = valY;
}
}
// int DataSourceCameraVShaped::getValueAtk(bool fixed){

11
src/drivecontroller.cpp
View File

@ -30,10 +30,11 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
output = 0;
setpoint = 0;
pid = new PID(&input, &output, &setpoint, KP, KI, KD, REVERSE);
pid = new PID(&input, &output, &setpoint, KP, KI, KD, 1,DIRECT);
pid->SetSampleTime(1.5);
pid->SetDerivativeLag(1);
pid->setAngleWrap(true);
pid->SetDerivativeLag(2);
pid->SetOutputLimits(-255,255);
pid->SetMode(AUTOMATIC);
@ -46,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
vyn = 0;
}
void DriveController::prepareDrive(int dir, int speed, int tilt){
void DriveController::prepareDrive(int dir, int speed, int tilt=0){
pDir = dir;
pSpeed = speed;
pTilt = tilt;
@ -81,7 +82,7 @@ void DriveController::drive(int dir, int speed, int tilt){
speed4 = -(speed2);
// calcola l'errore di posizione rispetto allo 0
delta = CURRENT_DATA_READ.IMUAngle;
delta = compass->getValue();
if(delta > 180) delta = delta - 360;
input = delta;
@ -89,7 +90,7 @@ void DriveController::drive(int dir, int speed, int tilt){
pid->Compute();
pidfactor = output;
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;

5
src/goalie.cpp
View File

@ -2,6 +2,7 @@
#include "sensors.h"
#include "vars.h"
#include "status_vector.h"
#include "math.h"
#include "positionsys_zone.h"
Goalie::Goalie() : Game() {
@ -38,7 +39,7 @@ void Goalie::goalie(int plusang) {
else dir = dir;
storcimentoPorta();
if(ball->distance > 190 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, 0);
if(ball->distance > 185 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
else {
drive->prepareDrive(dir, 350, 0);
cstorc = 0;
@ -48,7 +49,7 @@ void Goalie::goalie(int plusang) {
void Goalie::storcimentoPorta() {
if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9;
else if (CURRENT_DATA_READ.angleAtkFix <= 350 && CURRENT_DATA_READ.angleAtkFix >= 270) cstorc-=9;
else if (CURRENT_DATA_READ.angleAtkFix <= -10 && CURRENT_DATA_READ.angleAtkFix >= -90) cstorc-=9;
// else cstorc *= 0.7;
cstorc = constrain(cstorc, -45, 45);
}

1
src/main.cpp
View File

@ -24,6 +24,7 @@ void loop() {
keeper->play(role==0);
// Last thing to do: movement and update status vector
drive->prepareDrive(0,0,0);
drive->drivePrepared();
updateStatusVector();
}

6
src/positionsys_zone.cpp
View File

@ -374,9 +374,9 @@ void PositionSysZone::testLogicZone(){
void PositionSysZone::goCenter() {
if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
else drive->prepareDrive(0, 0, 0);
// if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
// else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
// else drive->prepareDrive(0, 0, 0);
/* if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
else drive->prepareDrive(0, 0, 0); */

4
src/sensors.cpp
View File

@ -15,7 +15,7 @@ void initSensors(){
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));
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCameraConic(&Serial2, 19200);
camera = new DataSourceCameraVShaped(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs);
bt = new DataSourceBT(&Serial3, 115200);
}
@ -26,6 +26,6 @@ void updateSensors(){
compass->update();
ball->update();
camera->update();
// camera->update();
usCtrl->update();
}

4
utility/OpenMV/conic_eff.py
View File

@ -39,8 +39,8 @@ blue_led.on()
thresholds = [ (0, 99, -16, 19, 13, 85), # thresholds yellow goal
(26, 52, -8, 19, -49, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (26, 74, -11, 6, 17, 50), # thresholds yellow goal
(12, 44, -34, 42, -105, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)