added excel sheet logic

Valor6800 · Apr 2, 2022 · d0f592e · d0f592e
1 parent b9c8681
commit d0f592e
Show file tree

Hide file tree

Showing 4 changed files with 62 additions and 19 deletions.
diff --git 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<double>();
     // if (state.bButtonPressed) {

diff --git 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<double>();
-    // state.cachedVY = drivetrain->getKinematics().ToChassisSpeeds().vy.to<double>();
-    // state.cachedVT = drivetrain->getKinematics().ToChassisSpeeds().omega.to<double>();
-
     double tv = shooter->state.tv;
 
+    double robotHeading = drivetrain->getPose_m().Rotation().Degrees().to<double>();
+    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<double>();
+
+        state.target += tMinusJ(robotHeading, turretPos, jx, jy);
+
+        state.cachedHeading = robotHeading;
         state.cachedX = drivetrain->getPose_m().X().to<double>();
         state.cachedY = drivetrain->getPose_m().Y().to<double>();
-        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<double>() + 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
@@ -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
@@ -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;