From 4397d602cd8583d6d0c236e2a4e0be261fcf8d6d Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 28 May 2021 20:53:35 +0200 Subject: [PATCH] striker: follow the ball, pick it and score Our roller is about to fall apart and i have no specific sensor to detect the ball, which means in perfect conditions it works but it really doesn't in real world conditions Also we need to ameliorate the communication protocol between teensy and 32u4 in order to not lose precision --- include/motors_movement/drivecontroller.h | 2 +- include/motors_movement/roller.h | 2 +- include/sensors/data_source_ball.h | 2 +- include/strategy_roles/striker.h | 5 +- .../systems/lines/linesys_camera_recovery.h | 49 ++++ lib/ArduinoPIDLibraryFlavio/PID_v2.cpp | 145 ++++-------- lib/ArduinoPIDLibraryFlavio/PID_v2.h | 36 +-- src/main.cpp | 18 +- src/motors_movement/drivecontroller.cpp | 4 +- src/sensors/data_source_ball.cpp | 9 +- src/strategy_roles/striker.cpp | 52 +++-- src/system/lines/linesys_camera_recovery.cpp | 117 ++++++++++ src/system/positions/positionsys_camera.cpp | 2 - utility/OpenMV/conic_eff_h7.py.autosave | 219 ------------------ 14 files changed, 276 insertions(+), 386 deletions(-) create mode 100644 include/systems/lines/linesys_camera_recovery.h create mode 100644 src/system/lines/linesys_camera_recovery.cpp delete mode 100644 utility/OpenMV/conic_eff_h7.py.autosave diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index c043e57..c65a51b 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -10,7 +10,7 @@ //BEST NUMBERS YET //USE MOVING AVERAGE AND ANGLE WRAP -#define KP 0.8 +#define KP 1.35 #define KI 0.0 #define KD 0.025 diff --git a/include/motors_movement/roller.h b/include/motors_movement/roller.h index 88ee39b..5c57ce4 100644 --- a/include/motors_movement/roller.h +++ b/include/motors_movement/roller.h @@ -2,7 +2,7 @@ #include "ESC.h" -#define ROLLER_DEFAULT_SPEED 1200 +#define ROLLER_DEFAULT_SPEED 1300 class Roller{ public: diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index 5abc015..aef29b0 100644 --- a/include/sensors/data_source_ball.h +++ b/include/sensors/data_source_ball.h @@ -4,7 +4,7 @@ #define MOUTH_MIN_ANGLE 340 #define MOUTH_MAX_ANGLE 20 -#define MOUTH_DISTANCE 115 +#define MOUTH_DISTANCE 100 #define MOUTH_MAX_DISTANCE 140 class DataSourceBall : public DataSource{ diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 720aefe..9bfd37a 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -20,9 +20,10 @@ class Striker : public Game{ void init() override; void striker(); int tilt(); + float ballTilt(); - int atk_speed, atk_direction, atk_tilt; - float cstorc; + int atk_speed, atk_direction; + float atk_tilt, ball_tilt; bool gotta_tilt; }; diff --git a/include/systems/lines/linesys_camera_recovery.h b/include/systems/lines/linesys_camera_recovery.h new file mode 100644 index 0000000..d0dc6d7 --- /dev/null +++ b/include/systems/lines/linesys_camera_recovery.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include "behaviour_control/ds_ctrl.h" +#include "systems/systems.h" + +#include "vars.h" + +#define S1O A7 +#define S1I A6 +#define S2O A2 +#define S2I A3 +#define S3I A9 +#define S3O A8 +#define S4I A0 +#define S4O A1 + +#define LINE_THRESH_CAM_REC 350 +#define EXIT_TIME_REC 300 + +#define X_LIMIT 4 +#define Y_LIMIT 5 + +class LineSysCameraRecovery : public LineSystem{ + + public: + LineSysCameraRecovery(); + LineSysCameraRecovery(vector in_, vector out); + + void update() override; + void test() override; + void outOfBounds(); + void checkLineSensors(); + void striker(); + + bool isInLimitX(); + bool isInLimitY(); + + private: + vector in, out; + DataSource* ds; + bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; + int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; + unsigned long exitTimer; + int outDir, outVel; + byte linesens; + int outX, outY; +}; \ No newline at end of file diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp index 3ae1c52..9c33028 100644 --- a/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp @@ -17,12 +17,12 @@ * The parameters specified here are those for for which we can't set up * reliable defaults, so we need to have the user set them. ***************************************************************************/ -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int POn, int ControllerDirection) +PID::PID(double* Input_, double* Output_, double* Setpoint_, + double Kp, double Ki, double Kd, int ControllerDirection) { - myOutput = Output; - myInput = Input; - mySetpoint = Setpoint; + Output = Output_; + Input = Input_; + Setpoint = Setpoint_; inAuto = false; PID::SetOutputLimits(0, 255); //default output limit corresponds to @@ -31,24 +31,11 @@ PID::PID(double* Input, double* Output, double* Setpoint, SampleTime = 100; //default Controller Sample Time is 0.1 seconds PID::SetControllerDirection(ControllerDirection); - PID::SetTunings(Kp, Ki, Kd, POn); + PID::SetTunings(Kp, Ki, Kd); lastTime = millis()-SampleTime; } -/*Constructor (...)********************************************************* - * To allow backwards compatability for v1.1, or for people that just want - * to use Proportional on Error without explicitly saying so - ***************************************************************************/ - -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int ControllerDirection) - :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) -{ - -} - - /* Compute() ********************************************************************** * This, as they say, is where the magic happens. this function should be called * every time "void loop()" executes. the function will decide for itself whether a new @@ -57,55 +44,38 @@ PID::PID(double* Input, double* Output, double* Setpoint, **********************************************************************************/ bool PID::Compute() { - if(!inAuto) return false; + if(!inAuto) return; unsigned long now = millis(); - unsigned long timeChange = (now - lastTime); + int timeChange = (now - lastTime); if(timeChange>=SampleTime) { /*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 error = *Setpoint - *Input; - double dInput = (input - lastInput); - outputSum+= (ki * error); - - /*Add Proportional on Measurement, if P_ON_M is specified*/ - if(!pOnE) outputSum-= kp * dInput; - - if(outputSum > outMax) outputSum= outMax; - else if(outputSum < outMin) outputSum= outMin; - - /*Add Proportional on Error, if P_ON_E is specified*/ - double output; - if(pOnE) output = kp * error; - else output = 0; - - if(kd_lagpam <=1){ - /*Compute Rest of PID Output*/ - filteredDerivative =(1.0-kd_lagpam)*filteredDerivative - + (kd_lagpam)*dInput; + if(angleWrap){ + if(error > 180) error = error - 360; + else if(error < -180) error = error + 360; } - else{ - filteredDerivative = maf.process(dInput); - } - - output += outputSum - kd * filteredDerivative; - if(output > outMax) output = outMax; - else if(output < outMin) output = outMin; - *myOutput = output; + ITerm+= (ki * error); + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + double dInput = (*Input - lastInput); + if(angleWrap){ + if(dInput > 180) dInput = dInput - 360; + else if(dInput < -180) dInput = dInput + 360; + } + + /*Compute PID Output*/ + *Output = kp * error + ITerm- kd * dInput; + if(*Output > outMax) *Output = outMax; + else if(*Output < outMin) *Output = outMin; + /*Remember some variables for next time*/ - lastInput = input; + lastInput = *Input; lastTime = now; - return true; } - else return false; } /* SetTunings(...)************************************************************* @@ -113,20 +83,15 @@ bool PID::Compute() * it's called automatically from the constructor, but tunings can also * be adjusted on the fly during normal operation ******************************************************************************/ -void PID::SetTunings(double Kp, double Ki, double Kd, int POn) +void PID::SetTunings(double Kp, double Ki, double Kd) { - if (Kp<0 || Ki<0 || Kd<0) return; - - pOn = POn; - pOnE = POn == P_ON_E; - - dispKp = Kp; dispKi = Ki; dispKd = Kd; - - double SampleTimeInSec = ((double)SampleTime)/1000; +if (Kp<0 || Ki<0|| Kd<0) return; + + double SampleTimeInSec = ((double)SampleTime)/1000; kp = Kp; ki = Ki * SampleTimeInSec; kd = Kd / SampleTimeInSec; - + if(controllerDirection ==REVERSE) { kp = (0 - kp); @@ -135,13 +100,6 @@ void PID::SetTunings(double Kp, double Ki, double Kd, int POn) } } -/* SetTunings(...)************************************************************* - * Set Tunings using the last-rembered POn setting - ******************************************************************************/ -void PID::SetTunings(double Kp, double Ki, double Kd){ - SetTunings(Kp, Ki, Kd, pOn); -} - /* SetSampleTime(...) ********************************************************* * sets the period, in Milliseconds, at which the calculation is performed ******************************************************************************/ @@ -167,18 +125,15 @@ void PID::SetSampleTime(int NewSampleTime) **************************************************************************/ void PID::SetOutputLimits(double Min, double Max) { - if(Min >= Max) return; + if(Min > Max) return; outMin = Min; outMax = Max; - - if(inAuto) - { - if(*myOutput > outMax) *myOutput = outMax; - else if(*myOutput < outMin) *myOutput = outMin; - - if(outputSum > outMax) outputSum= outMax; - else if(outputSum < outMin) outputSum= outMin; - } + + if(*Output > outMax) *Output = outMax; + else if(*Output < outMin) *Output = outMin; + + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; } /* SetMode(...)**************************************************************** @@ -189,9 +144,9 @@ void PID::SetOutputLimits(double Min, double Max) void PID::SetMode(int Mode) { bool newAuto = (Mode == AUTOMATIC); - if(newAuto && !inAuto) + if(newAuto == !inAuto) { /*we just went from manual to auto*/ - PID::Initialize(); + Initialize(); } inAuto = newAuto; } @@ -202,10 +157,10 @@ void PID::SetMode(int Mode) ******************************************************************************/ void PID::Initialize() { - outputSum = *myOutput; - lastInput = *myInput; - if(outputSum > outMax) outputSum = outMax; - else if(outputSum < outMin) outputSum = outMin; + lastInput = *Input; + ITerm = *Output; + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; } /* SetControllerDirection(...)************************************************* @@ -216,15 +171,13 @@ void PID::Initialize() ******************************************************************************/ void PID::SetControllerDirection(int Direction) { - if(inAuto && Direction !=controllerDirection) - { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } controllerDirection = Direction; } +void PID::setAngleWrap(bool val){ + angleWrap = val; +} + /* Status Funcions************************************************************* * Just because you set the Kp=-1 doesn't mean it actually happened. these * functions query the internal state of the PID. they're here for display diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.h b/lib/ArduinoPIDLibraryFlavio/PID_v2.h index 2f93b2d..f7e9025 100644 --- a/lib/ArduinoPIDLibraryFlavio/PID_v2.h +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.h @@ -9,7 +9,6 @@ class PID { - public: //Constants used in some of the functions below @@ -17,8 +16,6 @@ class PID #define MANUAL 0 #define DIRECT 0 #define REVERSE 1 - #define P_ON_M 0 - #define P_ON_E 1 //commonly used functions ************************************************************************** PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and @@ -55,18 +52,7 @@ class PID void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which // the PID calculation is performed. default is 100 - void SetDerivativeLag(double val){ - kd_lagpam = val; - } - - void setAngleWrap(bool a){ - angleWrap = a; - } - - double getDerivative(){ - return filteredDerivative; - } - + void setAngleWrap(bool a); //Display functions **************************************************************** double GetKp(); // These functions query the pid for interal values. @@ -84,24 +70,22 @@ class PID double dispKd; // double kp; // * (P)roportional Tuning Parameter - double ki; // * (I)ntegral Tuning Parameter - double kd; // * (D)erivative Tuning Parameter - double filteredDerivative; - double kd_lagpam = 1; //* 0.15 to 0.35 + double ki; // * (I)ntegral Tuning Parameter + double kd; // * (D)erivative Tuning Parameter int controllerDirection; - int pOn; - double *myInput; // * Pointers to the Input, Output, and Setpoint variables - double *myOutput; // This creates a hard link between the variables and the - double *mySetpoint; // PID, freeing the user from having to constantly tell us - // what these values are. with pointers we'll just know. + double *Input; // * Pointers to the Input, Output, and Setpoint variables + double *Output; // This creates a hard link between the variables and the + double *Setpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. unsigned long lastTime; - double outputSum, lastInput; + double lastInput, ITerm; unsigned long SampleTime; double outMin, outMax; - bool inAuto, pOnE, angleWrap; + bool inAuto; + bool angleWrap; }; #endif diff --git a/src/main.cpp b/src/main.cpp index 679119b..2bb8c07 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,12 +13,12 @@ void updateRoller(); TestMenu* testmenu; +#define BALL_IN A13 + bool striker_condition = false; bool keeper_condition = false; void setup() { - pinMode(BUZZER, OUTPUT); - tone(BUZZER, 220, 250); delay(1500); DEBUG.begin(115200); @@ -49,19 +49,17 @@ void setup() { void loop() { updateSensors(); + drive->resetDrive(); - striker_condition = role == HIGH; + // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike; + keeper_condition = role == LOW; + striker->play(1); - // if(role) precision_shooter->play(1); - // else pass_and_shoot->play(1); + testmenu->testMenu(); - // keeper_condition = role == LOW; - // keeper->play(keeper_condition); - // testmenu->testMenu(); - - // // Last thing to do: movement and update status vector + // Last thing to do: movement and update status vector drive->drivePrepared(); updateStatusVector(); } \ No newline at end of file diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 930d057..a2aa010 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -26,11 +26,11 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) output = 0; setpoint = 0; - pid = new PID(&input, &output, &setpoint, KP, KI, KD, 1,DIRECT); + pid = new PID(&input, &output, &setpoint, KP, KI, KD,DIRECT); pid->SetSampleTime(2.5); pid->setAngleWrap(true); - pid->SetDerivativeLag(2); + // pid->SetDerivativeLag(2); pid->SetOutputLimits(-255,255); pid->SetMode(AUTOMATIC); diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index c95b498..548e11e 100644 --- a/src/sensors/data_source_ball.cpp +++ b/src/sensors/data_source_ball.cpp @@ -14,9 +14,8 @@ void DataSourceBall :: postProcess(){ ballSeen = distance == 255 ? 0 : 1; }else{ angle = value * 2; - int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? 360 -CURRENT_DATA_READ.IMUAngle : CURRENT_DATA_READ.IMUAngle; - int ballAngle = angle > 180 ? 360 -angle : angle; - angleFix = (ballAngle-imuAngle+360)%360; + int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; + angleFix = (angle+imuAngle+360)%360; } CURRENT_INPUT_WRITE.ballByte = value; CURRENT_DATA_WRITE.ballAngle = angle; @@ -45,9 +44,9 @@ bool DataSourceBall::isInFront(){ } bool DataSourceBall::isInMouth(){ - return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE); + return isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE; } bool DataSourceBall::isInMouthMaxDistance(){ - return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE); + return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE; } \ No newline at end of file diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index bfe7c56..33c1d39 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -22,7 +22,6 @@ void Striker::init() atk_speed = 0; atk_direction = 0; atk_tilt = 0; - cstorc = 0; gotta_tilt = false; } @@ -31,38 +30,49 @@ void Striker::realPlay() { if (CURRENT_DATA_READ.ballSeen) this->striker(); - else + else{ ps->goCenter(); + roller->speed(roller->MIN); + } } +float ctilt = 0; +unsigned long ttilt = 0; + + void Striker::striker(){ //seguo palla - int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG; - - if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus - if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180 - else ball_degrees2 = ball_deg; - - if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo - else dir = ball_deg - plusang; //se sto nel negativo sottraggo - - dir = (dir + 360) % 360; - drive->prepareDrive(dir, MAX_VEL_HALF, tilt()); - if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); else roller->speed(roller->MIN); - + + int ball_angle = CURRENT_DATA_READ.ballAngleFix; + if(ball_angle > 180) ball_angle -= 360; + + if(!ball->isInMouth())ttilt=millis(); + + // int tmp_ball_tilt = (0.25f*ball_angle+old_ball_Angle*0.75f); + // ball_tilt = ball->isInMouth() ? ball_angle : ballTilt(); + + // // drive->prepareDrive(0,30,ball->isInMouth() && roller->roller_armed ? tilt() : ballTilt()); + drive->prepareDrive(0,30, millis() - ttilt > 250 ? tilt(): ball_angle); + + // old_ball_Angle = ball_angle; + // old_ball_tilt = (int) ball_tilt; } -int Striker::tilt() { - if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true; - else gotta_tilt = false; +float Striker::ballTilt(){ + if(CURRENT_DATA_READ.ballAngle > 180 && CURRENT_DATA_READ.ballAngle < 340) ball_tilt -= 0.15f; + else if(CURRENT_DATA_READ.ballAngle <= 180 && CURRENT_DATA_READ.ballAngle > 20) ball_tilt += 0.15f; + return ball_tilt; +} - if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) { + +int Striker::tilt() { + if(CURRENT_DATA_READ.atkSeen && (CURRENT_DATA_READ.ballAngleFix > 340 || CURRENT_DATA_READ.ballAngleFix < 20)){ + atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45); + }else{ atk_tilt *= 0.8; if(atk_tilt <= 10) atk_tilt = 0; - }else{ - atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45); } return atk_tilt; diff --git a/src/system/lines/linesys_camera_recovery.cpp b/src/system/lines/linesys_camera_recovery.cpp new file mode 100644 index 0000000..3d495a6 --- /dev/null +++ b/src/system/lines/linesys_camera_recovery.cpp @@ -0,0 +1,117 @@ +#include "systems/lines/linesys_camera_recovery.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "strategy_roles/games.h" +#include "behaviour_control/status_vector.h" + +LineSysCameraRecovery::LineSysCameraRecovery(){} +LineSysCameraRecovery::LineSysCameraRecovery(vector in_, vector out_){ + this->in = in_; + this->out = out_; + + fboundsX = false; + fboundsY = false; + slow = false; + + linesensOldX = 0; + linesensOldY = 0; + + tookLine = false; + + for(int i = 0; i < 4; i++){ + linetriggerI[i] = 0; + linetriggerO[i] = 0; + } + + exitTimer = 0; + linesens = 0; +} + + +void LineSysCameraRecovery ::update() { + inV = 0; + outV = 0; + tookLine = false; + + for(DataSource* d : in) d->readSensor(); + for(DataSource* d : out) d->readSensor(); + + for(auto it = in.begin(); it != in.end(); it++){ + i = it - in.begin(); + ds = *it; + linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; + } + for(auto it = out.begin(); it != out.end(); it++){ + i = it - out.begin(); + ds = *it; + linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; + } + + for(int i = 0; i < 4; i++){ + inV = inV | (linetriggerI[i] << i); + outV = outV | (linetriggerO[i] << i); + } + + if (inV > 0 || outV > 0) { + if(millis() - exitTimer > EXIT_TIME) { + fboundsX = true; + fboundsY = true; + } + exitTimer = millis(); + } + + linesens |= inV | outV; + outOfBounds(); +} + +void LineSysCameraRecovery::outOfBounds(){ + // digitalWriteFast(BUZZER, LOW); + if(fboundsX == true) { + if(linesens & 0x02) linesensOldX = 2; + else if(linesens & 0x08) linesensOldX = 8; + if(linesensOldX != 0) fboundsX = false; + } + if(fboundsY == true) { + if(linesens & 0x01) linesensOldY = 1; + else if(linesens & 0x04) linesensOldY = 4; + if(linesensOldY != 0) fboundsY = false; + } + + if (millis() - exitTimer < EXIT_TIME){ + CURRENT_DATA_READ.game->ps->goCenter(); + tookLine = true; + tone(BUZZER, 220.00, 250); + }else{ + // drive->canUnlock = true; + linesens = 0; + linesensOldY = 0; + linesensOldX = 0; + } +} + +void LineSysCameraRecovery::test(){ + update(); + DEBUG.print("In: "); + for(DataSource* d : in){ + d->update(); + DEBUG.print(d->getValue()); + DEBUG.print(" | "); + } + DEBUG.print(" |---| "); + DEBUG.print("Out: "); + for(DataSource* d : out){ + d->update(); + DEBUG.print(d->getValue()); + DEBUG.print(" | "); + } + DEBUG.println(); + for(int i = 0; i < 4; i++){ + DEBUG.print(linetriggerI[i]); + DEBUG.print(" | "); + DEBUG.println(linetriggerO[i]); + } + + DEBUG.println(inV); + DEBUG.println(outV); + DEBUG.println(); +} \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index d923267..882b82e 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -22,12 +22,10 @@ PositionSysCamera::PositionSysCamera() { X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); X->SetOutputLimits(-MAX_X, MAX_X); X->SetMode(AUTOMATIC); - X->SetDerivativeLag(1); X->SetSampleTime(2); Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); Y->SetOutputLimits(-MAX_Y,MAX_Y); Y->SetMode(AUTOMATIC); - Y->SetDerivativeLag(1); Y->SetSampleTime(2); filterDir = new ComplementaryFilter(0.35); diff --git a/utility/OpenMV/conic_eff_h7.py.autosave b/utility/OpenMV/conic_eff_h7.py.autosave deleted file mode 100644 index b910e83..0000000 --- a/utility/OpenMV/conic_eff_h7.py.autosave +++ /dev/null @@ -1,219 +0,0 @@ -# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020 -# Based on: -# color tracking - By: paolix - ven mag 18 2018 - -# Automatic RGB565 Color Tracking Example -# - -import sensor, image, time, pyb, math - -from pyb import UART -uart = UART(3,19200, timeout_char = 1000) - -START_BYTE = chr(105) #'i' -END_BYTE = chr(115) #'s' -BYTE_UNKNOWN = chr(116) #'t' - -y_found = False -b_found = False - -#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/ -def val_map(x, in_min, in_max, out_min, out_max): - x = int(x) - in_min = int(in_min) - in_max = int(in_max) - out_min = int(out_min) - out_max = int(out_max) - return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) - -# Check side -def isInLeftSide(img, x): - return x < img.width() / 2 - -def isInRightSide(img, x): - return x > img.width() / 2 - - -# LED Setup ################################################################## -red_led = pyb.LED(1) -green_led = pyb.LED(2) -blue_led = pyb.LED(3) - -red_led.off() -green_led.off() -blue_led.on() -############################################################################## - - -thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) - - - - -roi = (50, 0, 250, 200) - -# Camera Setup ############################################################### -'''sensor.reset()xxxx -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QVGA) -sensor.skip_frames(time = 2000) -sensor.set_auto_gain(False) # must be turned off for color tracking -sensor.set_auto_whitebal(False) # must be turned off for color tracking -sensor.set_auto_exposure(False, 10000) vbc -#sensor.set_backlight(1) -#sensor.set_brightness(+2 ) -#sensor.set_windowing(roi) -clock = time.clock()''' - -sensor.reset() -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QVGA) -#sensor.set_windowing(roi) -sensor.set_contrast(0) -sensor.set_saturation(2) -sensor.set_brightness(3) -sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -1.804)) -sensor.set_auto_exposure(False, 6576) -#sensor.set_auto_gain(False, gain_db=8.78) -sensor.skip_frames(time = 300) - -clock = time.clock() -############################################################################## - - -while(True): - clock.tick() - - print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db())) - - blue_led.off() - - y_found = False - b_found = False - - tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata - tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata - - img = sensor.snapshot() - for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True): - img.draw_rectangle(blob.rect()) - #img.draw_cross(blob.cx(), blob.cy()) - - if (blob.code() == 1): - tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - y_found = True - if (blob.code() == 2): - tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - b_found = True - - tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste - tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste - - ny = len(tt_yellow) - nb = len(tt_blue) - - - #Formulas to compute position of points, considering that the H7 is rotated by a certain angle - #x = y-offset - #y = offset - x - - #Compute everything related to Yellow First - - y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1] - - - y_cx = int(y1_cy - img.height() / 2) - y_cy = int(img.width() / 2 - y1_cx) - - - #Normalize data between 0 and 100 - if y_found == True: - img.draw_cross(y1_cx, y1_cy) - - y_cx = val_map(y_cx, -img.height() / 2, img.height() / 2, 100, 0) - y_cy = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 100) - #Prepare for send as a list of characters - s_ycx = chr(y_cx) - s_ycy = chr(y_cy) - else: - y_cx = BYTE_UNKNOWN - y_cy = BYTE_UNKNOWN - #Prepare for send as a list of characters - s_ycx = y_cx - s_ycy = y_cy - - - - #Compute everything relative to Blue - '''Given the light situation in our lab and given that blue is usually harder to spot than yellow, we need to check it we got - a blue blob that is in the same side of the ground as the yellow one, if so, discard it and check a new one - ''' - - b_cx = BYTE_UNKNOWN - b_cy = BYTE_UNKNOWN - #Prepare for send as a list of characters - s_bcx = b_cx - s_bcy = b_cy - - if b_found == True: - for i in range(nb-1, 0,-1): - b_area, b1_cx, b1_cy, b_code = tt_blue[i] - if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): - - img.draw_cross(b1_cx, b1_cy) - - b_cx = int(b1_cy - img.height() / 2) - b_cy = int(img.width() / 2 - b1_cx) - - #print("before :" + str(b_cx) + " " + str(b_cy)) - - b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0) - b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100) - - #print("after :" + str(b_cx) + " " + str(b_cy)) - - #Prepare for send as a list of characters - s_bcx = chr(b_cx) - s_bcy = chr(b_cy) - - '''index = 1 - if b_found == True: - while nb-index >= 0: - b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index] - - index += 1 - # If the two blobs are on opposide side of the field, everything is good - if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): - - img.draw_cross(b1_cx, b1_cy) - - b_cx = int(b1_cy - img.height() / 2) - b_cy = int(img.width() / 2 - b1_cx) - - print("before :" + str(b_cx) + " " + str(b_cy)) - b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0) - b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100) - - print("after :" + str(b_cx) + " " + str(b_cy)) - - #Prepare for send as a list of characters - s_bcx = chr(b_cx) - s_bcy = chr(b_cy) - - break - else: - b_cx = BYTE_UNKNOWN - b_cy = BYTE_UNKNOWN - #Prepare for send as a list of characters - s_bcx = b_cx - s_bcy = b_cy''' - - #print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy)) - - uart.write(START_BYTE) - uart.write(s_bcx) - uart.write(s_bcy) - uart.write(s_ycx) - uart.write(s_ycy) - uart.write(END_BYTE)