diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 16810fa..518accc 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -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 diff --git a/include/sensors.h b/include/sensors.h index d71275e..1782296 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -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; diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp index 2ba9c4c..3ae1c52 100644 --- a/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp @@ -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); diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.h b/lib/ArduinoPIDLibraryFlavio/PID_v2.h index 6c7dd44..2f93b2d 100644 --- a/lib/ArduinoPIDLibraryFlavio/PID_v2.h +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.h @@ -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 diff --git a/src/data_source_camera_vshapedmirror.cpp b/src/data_source_camera_vshapedmirror.cpp index 63de4ca..690e148 100644 --- a/src/data_source_camera_vshapedmirror.cpp +++ b/src/data_source_camera_vshapedmirror.cpp @@ -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){ diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index fae1728..f9df6c5 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -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; diff --git a/src/goalie.cpp b/src/goalie.cpp index 5c95a0d..7659488 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -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); } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 50ea14e..6cf48f9 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -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(); } diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index b12ba93..7cc1f0e 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -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); */ diff --git a/src/sensors.cpp b/src/sensors.cpp index 1e72cdb..44f50bd 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -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(); } \ No newline at end of file diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 9a3c96d..675466e 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -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)