diff --git a/Competition/src/main/cpp/subsystems/Drivetrain.cpp b/Competition/src/main/cpp/subsystems/Drivetrain.cpp index 41ca261..da541e8 100644 --- a/Competition/src/main/cpp/subsystems/Drivetrain.cpp +++ b/Competition/src/main/cpp/subsystems/Drivetrain.cpp @@ -245,15 +245,24 @@ void Drivetrain::assignOutputs() units::radians_per_second_t rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_MAX_SPEED_RPS}; if(state.aButtonPressed){ - double magnitude = std::sqrt(std::pow(xSpeed, 2) + std::pow(ySpeed, 2)); - xSpeed /= magnitude; - ySpeed /= magnitude; - xSpeedMPS = units::meters_per_second_t{xSpeed}; - ySpeedMPS = units::meters_per_second_t{ySpeed}; - if(rot != 0){ - int sign = std::signbit(rot) == 0 ? 1 : -1; - rotRPS = units::radians_per_second_t{SwerveConstants::ROTATION_SLOW_SPEED_RPS}; - } + xSpeedMPS = units::meters_per_second_t{xSpeed * SwerveConstants::DRIVE_SLOW_SPEED_MPS}; + ySpeedMPS = units::meters_per_second_t{ySpeed * SwerveConstants::DRIVE_SLOW_SPEED_MPS}; + rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS}; + + + // double magnitude = hypot(xSpeed, ySpeed); + // if (magnitude > 1) + // { + // xSpeed /= magnitude; + // ySpeed /= magnitude; + // xSpeedMPS = units::meters_per_second_t{xSpeed}; + // ySpeedMPS = units::meters_per_second_t{ySpeed}; + // } + + // if(rot != 0){ + // int sign = std::signbit(rot) == 0 ? 1 : -1; + // rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS}; + // } } // double heading = getPose_m().Rotation().Degrees().to(); // if (state.bButtonPressed) { diff --git a/Competition/src/main/cpp/subsystems/Feeder.cpp b/Competition/src/main/cpp/subsystems/Feeder.cpp index 2b77d3c..8698894 100644 --- a/Competition/src/main/cpp/subsystems/Feeder.cpp +++ b/Competition/src/main/cpp/subsystems/Feeder.cpp @@ -95,12 +95,13 @@ void Feeder::assessInputs() state.feederState = FeederState::FEEDER_DISABLE; } - // Calculate instantaneous current - calcCurrent(); + } void Feeder::analyzeDashboard() { + // Calculate instantaneous current + calcCurrent(); state.reversed = table->GetBoolean("Reverse Feeder?", false); state.intakeReverseSpeed = table->GetNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE); state.feederReverseSpeed = table->GetNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE); diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 6663412..04d8d09 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -16,6 +16,8 @@ TurretTracker::TurretTracker() : ValorSubsystem() void TurretTracker::init() { initTable("TurretTracker"); table->PutBoolean("Use Turret Shoot", false); + table->PutNumber("Joystick Multiplier", ShooterConstants::jMultiplier); + table->PutNumber("Delta Heading", 0); } void TurretTracker::setDrivetrain(Drivetrain *dt){ @@ -31,35 +33,59 @@ void TurretTracker::assessInputs() { } void TurretTracker::analyzeDashboard() { + state.jMultiplier = table->GetNumber("Joystick Multiplier", ShooterConstants::jMultiplier); +} + +double TurretTracker::tMinusJ(double robotHeading, double turretPos, double jx, double jy) +{ + double turretHeading = robotHeading - 90 + turretPos; + if (turretHeading < -180) turretHeading += 360; + if (turretHeading > 180) turretHeading -= 360; + + double tx = (shooter->state.distanceToHub) * cos(turretHeading * MathConstants::toRadians); + double ty = (shooter->state.distanceToHub) * sin(turretHeading * MathConstants::toRadians); + + jx *= state.jMultiplier; + jy *= state.jMultiplier; + + double rx = tx - jx; + double ry = ty - jy; + + double turretHeadingDesired = atan2(ry, rx); + double deltaHeading = turretHeading - (turretHeadingDesired * MathConstants::toDegrees); + if (deltaHeading < -180) deltaHeading += 360; + if (deltaHeading > 180) deltaHeading -= 360; + + table->PutNumber("Delta Heading", deltaHeading); + return -deltaHeading; } void TurretTracker::assignOutputs() { - // state.cachedVX = drivetrain->getKinematics().ToChassisSpeeds().vx.to(); - // state.cachedVY = drivetrain->getKinematics().ToChassisSpeeds().vy.to(); - // state.cachedVT = drivetrain->getKinematics().ToChassisSpeeds().omega.to(); - double tv = shooter->state.tv; + double robotHeading = drivetrain->getPose_m().Rotation().Degrees().to(); + double turretPos = shooter->turretEncoder.GetPosition(); + double jx = drivetrain->state.leftStickX; + double jy = -1 * (drivetrain->state.leftStickY); + if (tv == 1) { state.cachedTx = shooter->state.tx; // 0.75 = limeligh KP state.target = (-state.cachedTx * 0.75) + shooter->turretEncoder.GetPosition(); - - state.cachedHeading = drivetrain->getPose_m().Rotation().Degrees().to(); - state.cachedX = drivetrain->getPose_m().X().to(); - state.cachedY = drivetrain->getPose_m().Y().to(); - state.cachedTurretPos = shooter->turretEncoder.GetPosition(); - // state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos; - // atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(()), drivetrain->getPose_m().X()); + state.target += tMinusJ(robotHeading, turretPos, jx, jy); + state.cachedHeading = robotHeading; + state.cachedX = drivetrain->getPose_m().X().to(); + state.cachedY = drivetrain->getPose_m().Y().to(); + state.cachedTurretPos = turretPos; } else { if (table->GetBoolean("Use Turret Shoot", false)) - state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to() + state.cachedTurretPos - state.cachedTx; + state.target = -1 * robotHeading + state.cachedTurretPos - state.cachedTx; else - state.target = shooter->turretEncoder.GetPosition(); + state.target = turretPos; } if (state.target < -90) { diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 68e6d31..3211c2e 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -135,6 +135,8 @@ namespace ShooterConstants{ constexpr static double limelightAngle = 50; constexpr static double hubHeight = 2.64; constexpr static double limelightHeight = .6075; + + const static double jMultiplier = .4; constexpr static double flywheelKP1 = 0.088; //1 constexpr static double flywheelKI1 = 0; @@ -241,7 +243,7 @@ namespace FeederConstants{ constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0; constexpr static int CACHE_SIZE = 20; - constexpr static double JAM_CURRENT = 30; + constexpr static double JAM_CURRENT = 20; } namespace MathConstants{ diff --git a/Competition/src/main/include/subsystems/TurretTracker.h b/Competition/src/main/include/subsystems/TurretTracker.h index 615a6ab..07ff09f 100644 --- a/Competition/src/main/include/subsystems/TurretTracker.h +++ b/Competition/src/main/include/subsystems/TurretTracker.h @@ -12,7 +12,6 @@ #include "Drivetrain.h" #include "Shooter.h" - #ifndef TURRETTRACKER_H #define TURRETTRACKER_H @@ -29,6 +28,8 @@ class TurretTracker : public ValorSubsystem void analyzeDashboard(); void assignOutputs(); + double tMinusJ(double robotHeading, double turretPos, double jx, double jy); + struct x { double target; @@ -46,6 +47,8 @@ class TurretTracker : public ValorSubsystem double cachedTurretPos; + double jMultiplier; + } state;