From d0f592e7f73756a69810e88369490d9cd004a9ec Mon Sep 17 00:00:00 2001 From: rohitalamgari Date: Sat, 2 Apr 2022 13:40:03 -0500 Subject: [PATCH] added excel sheet logic --- .../src/main/cpp/subsystems/Drivetrain.cpp | 27 +++++++---- .../src/main/cpp/subsystems/TurretTracker.cpp | 47 +++++++++++++++---- Competition/src/main/include/Constants.h | 2 + .../main/include/subsystems/TurretTracker.h | 5 +- 4 files changed, 62 insertions(+), 19 deletions(-) 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/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp index 9f33a2a..f773668 100644 --- a/Competition/src/main/cpp/subsystems/TurretTracker.cpp +++ b/Competition/src/main/cpp/subsystems/TurretTracker.cpp @@ -16,6 +16,7 @@ TurretTracker::TurretTracker() : ValorSubsystem() void TurretTracker::init() { initTable("TurretTracker"); table->PutBoolean("Use Turret Shoot", false); + table->PutNumber("Joystick Multiplier", ShooterConstants::jMultiplier); } void TurretTracker::setDrivetrain(Drivetrain *dt){ @@ -31,31 +32,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; + + 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.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 = shooter->turretEncoder.GetPosition(); + 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 6caf194..deda816 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -128,6 +128,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.1; constexpr static double flywheelKI1 = 0; 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;