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 //PID Constants
#define KP 1.5 #define KP 1.5
#define KI 0.0 #define KI 0
#define KD 0.3 #define KD 0.1
#define UNLOCK_THRESH 800 #define UNLOCK_THRESH 800

2
include/sensors.h
View File

@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
s_extr DataSourceBNO055* compass; s_extr DataSourceBNO055* compass;
s_extr DataSourceBall* ball; s_extr DataSourceBall* ball;
s_extr DataSourceCameraConic* camera; s_extr DataSourceCameraVShaped* camera;
s_extr DriveController* drive; s_extr DriveController* drive;
s_extr DataSourceBT* bt; 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*/ /*Compute all the working error variables*/
double input = *myInput; double input = *myInput;
double error = *mySetpoint - input; double error = *mySetpoint - input;
if(angleWrap){
if(error < -179) error += 360;
if(error > 180) error -= 360;
}
double dInput = (input - lastInput); double dInput = (input - lastInput);
outputSum+= (ki * error); outputSum+= (ki * error);

6
lib/ArduinoPIDLibraryFlavio/PID_v2.h
View File

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

26
src/data_source_camera_vshapedmirror.cpp
View File

@ -88,22 +88,24 @@ void DataSourceCameraVShaped :: postProcess(){
pDef = valY * -1; 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; datavalid = 0;
cameraReady = 1; //attivo flag di ricezione pacchetto 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){ // 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; output = 0;
setpoint = 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->SetSampleTime(1.5);
pid->SetDerivativeLag(1); pid->setAngleWrap(true);
pid->SetDerivativeLag(2);
pid->SetOutputLimits(-255,255); pid->SetOutputLimits(-255,255);
pid->SetMode(AUTOMATIC); pid->SetMode(AUTOMATIC);
@ -46,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
vyn = 0; vyn = 0;
} }
void DriveController::prepareDrive(int dir, int speed, int tilt){ void DriveController::prepareDrive(int dir, int speed, int tilt=0){
pDir = dir; pDir = dir;
pSpeed = speed; pSpeed = speed;
pTilt = tilt; pTilt = tilt;
@ -81,7 +82,7 @@ void DriveController::drive(int dir, int speed, int tilt){
speed4 = -(speed2); speed4 = -(speed2);
// calcola l'errore di posizione rispetto allo 0 // calcola l'errore di posizione rispetto allo 0
delta = CURRENT_DATA_READ.IMUAngle; delta = compass->getValue();
if(delta > 180) delta = delta - 360; if(delta > 180) delta = delta - 360;
input = delta; input = delta;
@ -89,7 +90,7 @@ void DriveController::drive(int dir, int speed, int tilt){
pid->Compute(); pid->Compute();
pidfactor = output; pidfactor = -output;
speed1 += pidfactor; speed1 += pidfactor;
speed2 += pidfactor; speed2 += pidfactor;
speed3 += pidfactor; speed3 += pidfactor;

5
src/goalie.cpp
View File

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

1
src/main.cpp
View File

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

6
src/positionsys_zone.cpp
View File

@ -374,9 +374,9 @@ void PositionSysZone::testLogicZone(){
void PositionSysZone::goCenter() { void PositionSysZone::goCenter() {
if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 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 if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
else drive->prepareDrive(0, 0, 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); /* 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 if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
else drive->prepareDrive(0, 0, 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)); 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(); compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600); ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCameraConic(&Serial2, 19200); camera = new DataSourceCameraVShaped(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs); usCtrl = new DataSourceCtrl(dUs);
bt = new DataSourceBT(&Serial3, 115200); bt = new DataSourceBT(&Serial3, 115200);
} }
@ -26,6 +26,6 @@ void updateSensors(){
compass->update(); compass->update();
ball->update(); ball->update();
camera->update(); // camera->update();
usCtrl->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 thresholds = [ (26, 74, -11, 6, 17, 50), # thresholds yellow goal
(26, 52, -8, 19, -49, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (12, 44, -34, 42, -105, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)