controlle change

5 files changed, 26 insertions, 123 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 98824ae..017fa74 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,14 +4,14 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.util.IMUGyro;
 
 public class Constants 
 {
-    public static final GenericHID controller = new GenericHID(1);
+    public static final CommandXboxController controller = new CommandXboxController(0);
 
     public static final double DriveMaxSpeed = 5; // m/s
     public static final double DriveMaxAngular = Math.PI; // rad/s?
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7afe5f8..bdaae21 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,10 +4,6 @@
 
 package frc.robot;
 
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3580633..ed6afe9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,13 +4,10 @@
 
 package frc.robot;
 
-import javax.smartcardio.CommandAPDU;
-
 import edu.wpi.first.math.geometry.Pose2d;
-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.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.subsystems.Drivetrain;
 
@@ -18,7 +15,7 @@ public class RobotContainer
 {
     private final Drivetrain m_drive = new Drivetrain();
 
-    private final GenericHID m_controller = Constants.controller;
+    private final CommandXboxController m_controller = Constants.controller;
 
     public RobotContainer() 
     {
@@ -34,59 +31,37 @@ public class RobotContainer
     {
         m_drive.setDefaultCommand(m_drive.run(() -> 
         m_drive.tank(
-            deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
-            deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
+            deadzone(-0.09, 0.09, m_controller.getLeftY()) * Constants.DriveMaxSpeed,
+            deadzone(-0.09, 0.09, m_controller.getRightY()) * Constants.DriveMaxSpeed
         )));
 
         // debug controls
-        new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
+        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());
         })); 
-        new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> {
+        m_controller.b().onTrue(Commands.runOnce(() -> {
             System.out.println("Reset");
             m_drive.reset();
         })); 
 
-
-        new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8))
+        m_controller.x().and(m_controller.leftBumper())
             .onTrue(Commands.sequence(Commands.runOnce(() -> 
                     System.out.println("Quasistatic Forward")),
                 m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(7))
+        m_controller.x().and(m_controller.rightBumper())
             .onTrue(Commands.sequence(Commands.runOnce(() -> 
                     System.out.println("Quasistatic Reverse")),
                 m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-        new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(8))
+        m_controller.y().and(m_controller.leftBumper())
             .onTrue(Commands.sequence(Commands.runOnce(() -> 
                     System.out.println("Dynamic Forward")),
                 m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(7))
+        m_controller.y().and(m_controller.rightBumper())
             .onTrue(Commands.sequence(Commands.runOnce(() -> 
                     System.out.println("Dynamic Reverse")),
                 m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-                
-        // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.runOnce(() -> {
-        //     System.out.println("Running Quasistatis routine");
-        //     if(m_controller.getRawButton(8)) {
-        //         System.out.println("Reverse");
-        //         Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse));
-        //     } else {
-        //         System.out.println("Forward");
-        //         Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward));
-        //     }
-        // })); 
-        // new Trigger(() -> m_controller.getRawButton(4) && m_controller()).onTrue(() -> {
-        //     System.out.println("Running Dynamic routine");
-        //     if(m_controller.getRawButton(8)) {
-        //         System.out.println("Reverse");
-        //         return m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse);
-        //     } else {
-        //         System.out.println("Forward");
-        //         return m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward);
-        //     }
-        // }()); 
     }
 
     public Command getAutonomousCommand() 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index c149477..557c7e3 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -175,11 +175,20 @@ public class Drivetrain extends SubsystemBase
 
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
-        return m_sysIdRoutine.quasistatic(direction);
+        return Commands.race(m_sysIdRoutine.quasistatic(direction),
+            Commands.runOnce(() -> {
+                while(m_encoderL.getDistance() < 6 && 
+                      m_encoderR.getDistance() < 6);
+            }));
+        
     }
 
     public Command sysIdDynamic(SysIdRoutine.Direction direction)
     {
-        return m_sysIdRoutine.dynamic(direction);
+        return Commands.race(m_sysIdRoutine.dynamic(direction),
+        Commands.runOnce(() -> {
+            while(m_encoderL.getDistance() < 6 && 
+                  m_encoderR.getDistance() < 6);
+        }));
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 8d41c63..aa4563b 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -28,21 +28,17 @@ public class IMUGyro
     private final int GYRO_ADDR = 0x43;
     private final int GYRO_CONFIG = 0x1B;
  
-    private final int ACCL_ADDR = 0x3B;
-    private final int ACCL_CONFIG = 0x1C;
- 
     private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
-    private final double SCALE_ACCL = (1/16.384);
  
     public IMUGyro(I2C gyro) 
     {
         mpu = gyro;
         reset();
 
-        mpu.write(CONFIG_ADDR, 0b00000100);
-        mpu.write(SMPLRT_DIV, 0b00010011);
+        mpu.write(CONFIG_ADDR, 0b00000101);
+        // mpu.write(SMPLRT_DIV, 0b00010011);
         mpu.write(GYRO_CONFIG, 0b00011000);
-        mpu.write(ACCL_CONFIG, 0b00000000);
+        // mpu.write(ACCL_CONFIG, 0b00000000);
     }
  
     public void reset()
@@ -95,11 +91,6 @@ public class IMUGyro
         return scaledRates(GYRO_ADDR, SCALE_GYRO);
     }
  
-    private double[] acclRates()
-    {
-        return scaledRates(ACCL_ADDR, SCALE_ACCL);
-    }
- 
     private double[] scaledRates(int addr, double scale)
     {
         byte[] buffer = new byte[6];
@@ -114,73 +105,5 @@ public class IMUGyro
     {
         return ((first << 8) | second);
     }
-
-    // mahony quaternion
-    private double[] q = {1.0, 0.0, 0.0, 0.0};
-    // mahony integral term
-    private double[] i = new double[3];
-
-    private double[] fromQuaternion(double[] q)
-    {
-        return new double[] {
-            Math.atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2])),
-            Math.asin(2.0 * (q[0] * q[2] - q[1] * q[3])),
-            -Math.atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]))
-        };
-    }
-
-    private void mahony(double Ki, double Kp, double[] gRates, double[] aRates, double dt)
-    {
-        double gx = gRates[0], gy = gRates[1], gz = gRates[2];
-        double ax = aRates[0], ay = aRates[1], az = aRates[2];
-
-        double tmp = ax * ax + ay * ay + az * az;
-        if(tmp > 0.0) 
-        {
-            tmp = Math.sqrt(tmp);
-            ax /= tmp; ay /= tmp; az /= tmp;
-
-            // estimated direction of gravity in the body frame
-            double vx = q[1] * q[3] - q[0] * q[2];
-            double vy = q[0] * q[1] + q[2] * q[3];
-            double vz = q[0] * q[0] - 0.5f + q[3] * q[3];
-
-            // error is cross product
-            double ex = (ay * vz - az * vy);
-            double ey = (az * vx - ax * vz);
-            double ez = (ax * vy - ay * vx);
-
-            // integral feedback
-            if(Ki > 0.0) {
-                i[0] += Ki * ex * dt;
-                i[1] += Ki * ey * dt;
-                i[2] += Ki * ez * dt;
-                gx += i[0];  
-                gy += i[1];
-                gz += i[2];
-            }
-
-            gx += Kp * ex;
-            gy += Kp * ey;
-            gz += Kp * ez;            
-        }
-
-        // integrate quaternion
-        dt /= 2; // ??
-        gx *= dt; gy *= dt; gz *= dt;
-        
-        double qa = q[0], qb = q[1], qc = q[2], qd = q[3];
-        q[0] += (-qb * gx - qc * gy - qd * gz);
-        q[1] += (qa * gx + qc * gz - qd * gy);
-        q[2] += (qa * gy - qb * gz + qd * gx);
-        q[3] += (qa * gz + qb * gy - qc * gx);
-
-        // renormalize
-        double qnorm_factor = Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-        q[0] /= qnorm_factor;
-        q[1] /= qnorm_factor;
-        q[2] /= qnorm_factor;
-        q[3] /= qnorm_factor;
-    }
 }