Friday CMP

TripleHelixProgramming · May 11, 2023 · a1eec79 · a1eec79
1 parent b092fa8
commit a1eec79
Show file tree

Hide file tree

Showing 11 changed files with 2,295 additions and 2,265 deletions.
diff --git a/.Glass/glass.json b/.Glass/glass.json
@@ -40,6 +40,9 @@
           },
           "open": true
         },
+        "Vision": {
+          "open": true
+        },
         "open": true
       },
       "cadmia": {
@@ -80,7 +83,7 @@
     "Plot <0>": {
       "plots": [
         {
-          "height": 338,
+          "height": 0,
           "series": [
             {
               "color": [

diff --git a/autos/red-north-3cone-low/red-north-3cone-low_2_place8.json b/autos/red-north-3cone-low/red-north-3cone-low_2_place8.json
@@ -39,7 +39,7 @@
                     "waypoint_type": "custom",
                     "x": 11.65525392711544,
                     "x_constrained": true,
-                    "y": 4.73941277169068,
+                    "y": 4.612412771690679,
                     "y_constrained": true
                 },
                 {

diff --git a/autos/red-north-3cone-low/red-north-3cone-low_3_pick3.json b/autos/red-north-3cone-low/red-north-3cone-low_3_pick3.json
@@ -55,9 +55,9 @@
                     "velocity_y": 0.3,
                     "velocity_y_constrained": false,
                     "waypoint_type": "custom",
-                    "x": 10.389846639887583,
+                    "x": 10.010435516088627,
                     "x_constrained": true,
-                    "y": 3.6089814971674348,
+                    "y": 3.8508929249637114,
                     "y_constrained": true
                 },
                 {
@@ -73,9 +73,9 @@
                     "velocity_y": 0.0,
                     "velocity_y_constrained": true,
                     "waypoint_type": "custom",
-                    "x": 9.75918458474652,
+                    "x": 9.379773460947565,
                     "x_constrained": true,
-                    "y": 2.9700968481329495,
+                    "y": 3.212008275929226,
                     "y_constrained": true
                 }
             ]

diff --git a/autos/red-north-3cone-low/red-north-3cone-low_4_place7.json b/autos/red-north-3cone-low/red-north-3cone-low_4_place7.json
@@ -9,7 +9,7 @@
                 {
                     "angular_velocity": 0.0,
                     "angular_velocity_constrained": true,
-                    "control_interval_count": 20,
+                    "control_interval_count": 0,
                     "heading": -2.3420807274344924,
                     "heading_constrained": true,
                     "name": "pick",
@@ -19,9 +19,9 @@
                     "velocity_y": 0.0,
                     "velocity_y_constrained": true,
                     "waypoint_type": "custom",
-                    "x": 9.75918458474652,
+                    "x": 9.379773460947565,
                     "x_constrained": true,
-                    "y": 2.9700968481329495,
+                    "y": 3.212008275929226,
                     "y_constrained": true
                 },
                 {

diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -59,10 +59,14 @@ RobotContainer::RobotContainer(std::function<bool(void)> isDisabled)
         double rightXAxis = -m_driver.GetRawAxis(kZorroRightYAxis);
         double rightYAxis = -m_driver.GetRawAxis(kZorroRightXAxis);
         double leftXAxis = -m_driver.GetRawAxis(kZorroLeftXAxis);
+        double x = std::abs(rightXAxis) < 0.025 ? 0.0 : rightXAxis;
+        double y = std::abs(rightYAxis) < 0.025 ? 0.0 : rightYAxis;
+        double omega = std::abs(leftXAxis) < 0.025 ? 0.0 : leftXAxis;
         return m_drive.JoystickDrive(
-            std::abs(rightXAxis) < 0.025 ? 0.0 : rightXAxis,
-            std::abs(rightYAxis) < 0.025 ? 0.0 : rightYAxis,
-            std::abs(leftXAxis) < 0.025 ? 0.0 : leftXAxis, true, m_isBlue);
+            x,
+            y,
+            omega,
+            true, m_isBlue, m_superstructure.GetAbsoluteArmPosition().value());
       },
       {&m_drive}  // requirements
       ));

diff --git a/src/main/cpp/commands/autos/North3ConeLow.cpp b/src/main/cpp/commands/autos/North3ConeLow.cpp
@@ -57,9 +57,14 @@ North3ConeLow::North3ConeLow(SwerveDrive* drive, Superstructure* superstructure,
               WaitCommand(TrajectoryManager::GetTrajectory(
                               allianceSidePrefix + "north-3cone-low_2_place8")
                               .GetTotalTime() -
-                          0.1_s),
+                          0.2_s),
               InstantCommand(
-                  [superstructure]() { superstructure->Outtake(); }))),
+                  [superstructure]() { superstructure->m_flipConeMode = true; }))),
+      InstantCommand(
+                  [superstructure]() { superstructure->Outtake(); }),
+      WaitCommand(0.25_s),
+      InstantCommand(
+                  [superstructure]() { superstructure->m_flipConeMode = false; }),
       ParallelDeadlineGroup(
           SequentialCommandGroup(
               WaitCommand(0.1_s),

diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp
@@ -34,6 +34,7 @@
 #include <wpi/array.h>
 
 #include "Constants.hpp"
+#include "frc/kinematics/ChassisSpeeds.h"
 #include "util/log/DoubleTelemetryEntry.hpp"
 #include "util/log/TelemetryEntry.hpp"
 
@@ -139,7 +140,23 @@ void SwerveDrive::ResetOdometry(const Pose2d& pose) {
 
 void SwerveDrive::JoystickDrive(double joystickDrive, double joystickStrafe,
                                 double joystickRotate, bool fieldRelative,
-                                bool isBlue) {
+                                bool isBlue, double armPosition) {
+  double a1 = -5.0;
+  meter_t d1 = 16_in;
+  double a2 = 8.3;
+  meter_t d2 = 45_in;
+  double a3 = 30;
+  meter_t d3 = 65_in;
+
+  meter_t d;
+  if (armPosition < a2) {
+    double scale = (armPosition - a1) / (a2 - a1);
+    d = d1 + scale * (d2 - d1);
+  } else {
+    double scale = (armPosition - a2) / (a3 - a2);
+    d = d2 + scale * (d3 - d2);
+  }
+
   ChassisSpeeds speeds =
       fieldRelative
           ? ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -150,6 +167,7 @@ void SwerveDrive::JoystickDrive(double joystickDrive, double joystickStrafe,
           : ChassisSpeeds{joystickDrive * kMaxVelocityX,
                           joystickStrafe * kMaxVelocityY,
                           joystickRotate * kMaxVelocityAngular};
+  speeds = ChassisSpeeds{speeds.vx, speeds.vy - meters_per_second_t{speeds.omega.value()} * d.value(), speeds.omega};
   auto states = m_driveKinematics.ToSwerveModuleStates(speeds);
 
   // use most extreme axis as scale factor