Don't do closed loop

Gold856 · Jun 17, 2024 · 9e18f09 · 9e18f09
1 parent bc04dd9
commit 9e18f09
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 15 deletions.
diff --git a/...ibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/...ibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -90,8 +90,11 @@ frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
     units::meter_t distance) {
   auto profile =
       frc::TrapezoidProfile<units::meters>{{kMaxSpeed, kMaxAcceleration}};
+  frc::Timer timer{};
   return StartRun(
              [&] {
+               // Restart timer so profile setpoints start at the beginning
+               timer.Restart();
                // Store distance so we know the target distance for each encoder
                m_initialLeftDistance = GetLeftEncoderDistance();
                m_initialRightDistance = GetRightEncoderDistance();
@@ -101,14 +104,12 @@ frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
                // profile needs to look 20 milliseconds ahead for the next
                // setpoint
                auto leftSetpoint = profile.Calculate(
-                   kDt,
-                   {GetLeftEncoderDistance(),
-                    units::meters_per_second_t{m_leftLeader.GetEncoderRate()}},
+                   timer.Get(),
+                   {m_initialLeftDistance, 0_mps},
                    {m_initialLeftDistance + distance, 0_mps});
                auto rightSetpoint = profile.Calculate(
-                   kDt,
-                   {GetLeftEncoderDistance(),
-                    units::meters_per_second_t{m_rightLeader.GetEncoderRate()}},
+                   timer.Get(),
+                   {m_initialRightDistance, 0_mps},
                    {m_initialRightDistance + distance, 0_mps});
                SetDriveStates(leftSetpoint, rightSetpoint);
              })

diff --git a/.../java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/.../java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -140,7 +140,7 @@ public Command profiledDriveDistance(double distance) {
             () -> {
               // Current state never changes, so we need to use a timer to get the setpoints we need
               // to be at
-              var setpoint = profile.calculate(timer.get(), new State(), new State(3, 0));
+              var setpoint = profile.calculate(timer.get(), new State(), new State(distance, 0));
               setDriveStates(setpoint, setpoint);
             })
         .until(() -> profile.isFinished(0));
@@ -162,25 +162,28 @@ public Command dynamicProfiledDriveDistance(double distance) {
             new TrapezoidProfile.Constraints(
                 DriveConstants.kMaxSpeedMetersPerSecond,
                 DriveConstants.kMaxAccelerationMetersPerSecondSquared));
+                    var timer = new Timer();
     return startRun(
             () -> {
+              // Restart timer so profile setpoints start at the beginning
+              timer.restart();
               // Store distance so we know the target distance for each encoder
               m_initialLeftDistance = getLeftEncoderDistance();
               m_initialRightDistance = getRightEncoderDistance();
             },
             () -> {
-              // Current state matches the actual state of the bot, so the profile needs to look 20
-              // milliseconds ahead for the next setpoint
+              // Current state never changes, so we need to use a timer to get the setpoints we need
+              // to be at
               var leftSetpoint =
                   profile.calculate(
-                      DriveConstants.kDt,
-                      new State(getLeftEncoderDistance(), m_leftLeader.getEncoderRate()),
-                      new State(m_initialLeftDistance + 3, 0));
+                      timer.get(),
+                      new State(m_initialLeftDistance, 0),
+                      new State(m_initialLeftDistance + distance, 0));
               var rightSetpoint =
                   profile.calculate(
-                      DriveConstants.kDt,
-                      new State(getRightEncoderDistance(), m_rightLeader.getEncoderRate()),
-                      new State(m_initialRightDistance + 3, 0));
+                      timer.get(),
+                      new State(m_initialRightDistance, 0),
+                      new State(m_initialRightDistance + distance, 0));
               setDriveStates(leftSetpoint, rightSetpoint);
             })
         .until(() -> profile.isFinished(0));