useful message

3 files changed, 109 insertions, 17 deletions

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8a3f9b4..7afe5f8 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -58,17 +58,21 @@ public class Robot extends TimedRobot {
     }
   }
 
-  GenericHID controller = Constants.controller;
+  // GenericHID controller = Constants.controller;
   // CANSparkMax shooter_top = new CANSparkMax(21, MotorType.kBrushless);
   // CANSparkMax shooter_bottom = new CANSparkMax(22, MotorType.kBrushless);
-  CANSparkMax intakeL = new CANSparkMax(31, MotorType.kBrushless);
-  CANSparkMax intakeR = new CANSparkMax(32, MotorType.kBrushless);
+  // CANSparkMax intakeL = new CANSparkMax(31, MotorType.kBrushless);
+  // CANSparkMax intakeR = new CANSparkMax(32, MotorType.kBrushless);
 
   @Override
   public void teleopPeriodic() 
   {
-    intakeL.set(controller.getRawAxis(1));
-    intakeR.set(controller.getRawAxis(3));
+    // shooter_top.setInverted(true);
+    // shooter_bottom.setInverted(true);
+    // shooter_top.set(controller.getRawAxis(1));
+    // shooter_bottom.set(controller.getRawAxis(3));
+    // intakeL.set(controller.getRawAxis(1));
+    // intakeR.set(controller.getRawAxis(3));
   }
 
   @Override
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 64ef818..3580633 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,11 +4,14 @@
 
 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.sysid.SysIdRoutine;
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
@@ -29,16 +32,13 @@ public class RobotContainer
 
     private void configureBindings() 
     {
-        // m_drive.setDefaultCommand(m_drive.run(() -> 
-        // // m_drive.arcade(
-        // //     deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed),
-        // //     deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular)
-        // // ))); 
-        // 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
-        // )));
+        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
+        )));
 
+        // debug controls
         new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
             Pose2d pose = m_drive.pose2d();
             System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
@@ -48,6 +48,45 @@ public class RobotContainer
             System.out.println("Reset");
             m_drive.reset();
         })); 
+
+
+        new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8))
+            .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))
+            .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))
+            .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))
+            .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 a7fe75e..c149477 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -10,11 +10,23 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.proto.Trajectory;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Volts;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
 
 import frc.robot.Constants;
 import frc.robot.util.IMUGyro;
@@ -41,6 +53,34 @@ public class Drivetrain extends SubsystemBase
 
     private DifferentialDriveOdometry m_odometry;
 
+    // For system identification
+    private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
+    private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
+    private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+    private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
+        new SysIdRoutine.Config(), 
+        new SysIdRoutine.Mechanism(
+            (Measure<Voltage> volts) -> {
+                m_motorL_leader.set(-volts.in(Volts));
+                m_motorR_leader.set(-volts.in(Volts));
+            },
+            log -> {
+                log.motor("drive-left")
+                    .voltage(
+                        m_voltage.mut_replace(
+                            m_motorL_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                    )
+                    .linearPosition(m_distance.mut_replace(m_encoderL.getDistance(), Meters))
+                    .linearVelocity(m_velocity.mut_replace(m_encoderL.getRate(), MetersPerSecond));
+                log.motor("driver-right")
+                .voltage(
+                        m_voltage.mut_replace(
+                            m_motorR_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                    )
+                    .linearPosition(m_distance.mut_replace(m_encoderR.getDistance(), Meters))
+                    .linearVelocity(m_velocity.mut_replace(m_encoderR.getRate(), MetersPerSecond));
+            }, this));
+
     public Drivetrain()
     {
         m_gyro.reset();
@@ -95,8 +135,6 @@ public class Drivetrain extends SubsystemBase
 
     public Pose2d pose2d() 
     {
-        System.out.println("Distance: " + m_encoderL.getDistance());
-        System.out.println("Distance: " + m_encoderR.getDistance());
         return m_odometry.getPoseMeters();
     }
 
@@ -130,7 +168,18 @@ public class Drivetrain extends SubsystemBase
 
     public Command trajectory(Trajectory trajectory)
     {
+        // TOOD: Implement
         // return this.runOnce(...)   
         return Commands.none();
     }
+
+    public Command sysIdQausistatic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.dynamic(direction);
+    }
 }
\ No newline at end of file