From 839871ba03cc6d9865154216174183ff012fe113 Mon Sep 17 00:00:00 2001
From: kartofen <mladenovnasko0@gmail.com>
Date: Mon, 4 Mar 2024 21:35:41 +0200
Subject: sysid and trajectory things

---
 src/main/java/frc/robot/RobotContainer.java | 86 ++++++++++++++++++-----------
 1 file changed, 54 insertions(+), 32 deletions(-)

(limited to 'src/main/java/frc/robot/RobotContainer.java')

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cbaceea..87c4e60 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,16 +4,25 @@
 
 package frc.robot;
 
-import org.littletonrobotics.junction.Logger;
+import java.util.List;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
+    // private final Chain m_chain = new Chain();
 
     private final GenericHID m_controller = Constants.controller;
 
@@ -22,6 +31,20 @@ public class RobotContainer
         configureBindings();
     }
 
+    private double[] correctInputs(double in1, double in2)  
+    {
+        double p1 = processInput(in1);
+        double p2 = processInput(in2);
+
+        if(Math.signum(p1) != Math.signum(p2)) 
+            return new double[] {p1, p2};
+
+        double diff = Math.abs(p1 - p2) * Constants.InputSmoothing;
+
+        return (p1 > p2) ? new double[] {p1 - diff, p2 + diff}
+                         : new double[] {p1 + diff, p2 - diff};
+    }
+
     private double processInput(double val) 
     {  
         double input = deadzone(-0.06, 0.06, val);
@@ -35,37 +58,36 @@ public class RobotContainer
     }
 
     private void configureBindings() 
-    {
-         m_drive.setDefaultCommand(m_drive.run(() -> 
-         m_drive.tank(
-             processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
-             processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
-         )));
-
-        // m_drive.setDefaultCommand(m_drive.run(() -> {
-        //     Logger.recordOutput("inputL", processInput(m_controller.getRawAxis(2)));
-        //     Logger.recordOutput("inputR", processInput(m_controller.getRawAxis(1)));
-        // }));
-        // m_controller.b().onTrue(Commands.runOnce(() -> {
-        //     m_drive.reset();
-        // })); 
-
-        // m_controller.x().and(m_controller.leftBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Quasistatic Forward")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        // m_controller.x().and(m_controller.rightBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Quasistatic Reverse")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-        // m_controller.y().and(m_controller.leftBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Dynamic Forward")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        // m_controller.y().and(m_controller.rightBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Dynamic Reverse")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
+    {   
+        m_drive.setDefaultCommand(m_drive.run(() -> {
+            double[] inputs = correctInputs(
+                    m_controller.getRawAxis(2),
+                    m_controller.getRawAxis(1));
+            m_drive.tank(
+                inputs[0] * Constants.DriveMaxSpeed,
+                inputs[1] * Constants.DriveMaxSpeed);
+        }));
+
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // List.of(),
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            new Pose2d(3, 0, new Rotation2d(0)), 
+            m_drive.trajectoyConfig);
+
+        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+
+        // m_chain.setDefaultCommand(m_chain.run(() ->
+        // m_chain.atPercentage(
+        //     processInput(m_controller.getRawAxis(2))
+        // )));
+
+
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
+        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
+
     }
 
     public Command getAutonomousCommand() 
-- 
cgit v1.2.3