more subsystems

1 files changed, 41 insertions, 37 deletions

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ed6afe9..cbaceea 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,64 +4,68 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Pose2d;
+import org.littletonrobotics.junction.Logger;
+
+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.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
 
-    private final CommandXboxController m_controller = Constants.controller;
+    private final GenericHID m_controller = Constants.controller;
 
     public RobotContainer() 
     {
         configureBindings();
     }
 
-    private double deadzone(double low, double high, double val) {
+    private double processInput(double val) 
+    {  
+        double input = deadzone(-0.06, 0.06, val);
+        return input * input * input;
+    }
+
+    private double deadzone(double low, double high, double val)
+    {
         if(val > low && val < high) return 0.0;
-        return val;
+        return ((val > 0.0) ? val - high : val - low);
     }
 
     private void configureBindings() 
     {
-        m_drive.setDefaultCommand(m_drive.run(() -> 
-        m_drive.tank(
-            deadzone(-0.09, 0.09, m_controller.getLeftY()) * Constants.DriveMaxSpeed,
-            deadzone(-0.09, 0.09, m_controller.getRightY()) * Constants.DriveMaxSpeed
-        )));
+         m_drive.setDefaultCommand(m_drive.run(() -> 
+         m_drive.tank(
+             processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
+             processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
+         )));
 
-        // debug controls
-        m_controller.a().onTrue(Commands.runOnce(() -> {
-            Pose2d pose = m_drive.pose2d();
-            System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
-            System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
-        })); 
-        m_controller.b().onTrue(Commands.runOnce(() -> {
-            System.out.println("Reset");
-            m_drive.reset();
-        })); 
+        // 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_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)));
     }
 
     public Command getAutonomousCommand()