Merge pull request #49 from BadRobots1014/knoxville-comp

Knoxville comp

src/main/java/frc/robot/Constants.java

@@ -6,7 +6,6 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.XboxController.Button;
 
@@ -106,9 +105,12 @@ public static final class DriveConstants {
 
     public static final double kTeleMaxRadiansPerSec = Math.PI / 2; // TODO adjust max teleop speeds
     public static final double kFastTeleMaxRadiansPerSec = Math.PI;
+    public static final double kFasterTeleMaxRadiansPerSec = Math.PI;
+
     public static final double kTeleMaxMetersPerSec = 0.3;
-    public static final double kFastTeleMaxMetersPerSec = 0.9;
-    public static final double kNudgeSpeed = 0.3;
+    public static final double kFastTeleMaxMetersPerSec = 1.0;
+    public static final double kFasterTeleMaxMetersPerSec = 1.8;
+    public static final double kNudgeSpeed = 0.5;
 
     public static final Button kTestMotorButton = Button.kLeftBumper;
   }
@@ -133,25 +135,7 @@ public static final class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kSecondControllerPort = 1;
     public static final double kDriveDeadband = 0.02;
-  }
-
-  // TODO These are old and should be removed
-  @Deprecated
-  public static final class AutoConstants {
-
-    public static final double kMaxSpeedMetersPerSecond = 3;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
-    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
-    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
-
-    public static final double kPXController = 1;
-    public static final double kPYController = 1;
-    public static final double kPThetaController = 1;
-
-    // Constraint for the motion profiled robot angle controller
-    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
-        kMaxAngularSpeedRadiansPerSecond,
-        kMaxAngularSpeedRadiansPerSecondSquared);
+    public static final double kTriggerDeadband = 0.75;
   }
 
   public static final class ShooterConstants {
@@ -164,10 +148,10 @@ public static final class ShooterConstants {
     public static final double kBackShootPower = 1.0;
     public static final double kFrontIntakePower = -0.35;
     public static final double kBackIntakePower = -0.30;
-    public static final double kIndexIntakePower = .1;
+    public static final double kIndexIntakePower = 0.1;
     public static final double kIndexPower = -1.0;
     public static final double kWinchUpPower = 0.5;
-    public static final double kWinchDownPower = -1;
+    public static final double kWinchDownPower = -1.0;
 
     public static final double kWinchDeadBand = 0.05;
     public static final double kWinchUpPreset = 0;
@@ -177,8 +161,9 @@ public static final class ShooterConstants {
   public static final class ClimberConstants {
     public static final int kLeftClimberCanId = 61;
     public static final int kRightClimberCanId = 62;
-    public static final int kClimberUpPower = 1;
-    public static final int kClimberDownPower = -1;
+    public static final int kClimberUpPower = -1;
+    public static final int kClimberDownPower = 1;
+    public static final int kClimberMaxAmps = 200000;
   }
 
   public static final class LimelightConstants {

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,8 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -33,6 +35,12 @@ public void robotInit() {
     // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    // Starts recording to data log
+    DataLogManager.start();
+
+    // Record both DS control and joystick data
+    DriverStation.startDataLog(DataLogManager.getLog());
   }
 
   /**

src/main/java/frc/robot/RobotContainer.java

@@ -1,12 +1,12 @@
-// Copyright (c) FIRST and other WPILib contributors.
+// Copyright  (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
-
 import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -21,17 +21,19 @@
 import frc.robot.commands.WinchCommand;
 import frc.robot.commands.WinchPresetCommand;
 import frc.robot.commands.ZeroHeadingCommand;
+import frc.robot.commands.auto.DriveAutoCommand;
 import frc.robot.commands.auto.ShootAndDriveAutoCommand;
+import frc.robot.commands.auto.ShootAutoCommand;
 import frc.robot.commands.auto.TurnAndShootAutoCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
 import frc.robot.commands.ClimbCommand;
 import frc.robot.commands.IntakeCommand;
-import frc.robot.commands.ReleaseClimbersCommand;
 import frc.robot.commands.ResetWinchCommand;
 import frc.robot.commands.ShootCommand;
-import frc.robot.commands.ShooterCommand;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -49,11 +51,13 @@ public class RobotContainer {
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   private final SwerveSubsystem m_robotDrive = new SwerveSubsystem();
   private boolean fastMode = false;
+  private boolean fasterMode = false;
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 
   // Auto
   private final ShuffleboardTab m_tab;
   private SendableChooser<Command> m_chosenAuto = new SendableChooser<>();
+  private GenericEntry m_delay;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -66,30 +70,42 @@ public RobotContainer() {
             () -> getRightX(),
             DriveConstants.kFieldOriented,
             this::getFastMode,
+            this::getFasterMode,
             this::getPOV));
     m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY));
     m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed));
 
     // Auto chooser setup
     m_tab = Shuffleboard.getTab("Auto");
 
+    m_delay = m_tab.add("Delay", 0).getEntry();
+
     m_chosenAuto.setDefaultOption("Shoot and drive from middle",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d()));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it)
     m_chosenAuto.addOption("Shoot and drive from left",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(45))));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0)));
     m_chosenAuto.addOption("Shoot and drive from right",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-45))));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive, turn, and shoot from left",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 40));
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55, m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive, turn, and shoot from right",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -40));
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0)));
+    m_chosenAuto.addOption("Drive back only",
+      new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto
+    m_chosenAuto.addOption("Shoot only", 
+      new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4));
 
     m_tab.add(m_chosenAuto);
 
     // Configure the button bindings
     configureButtonBindings();
   }
 
+  public void robotInit() {
+    DataLogManager.start();
+    DriverStation.startDataLog(DataLogManager.getLog());
+  }
+
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by
@@ -105,6 +121,7 @@ private void configureButtonBindings() {
     new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro
       .whileTrue(new ZeroHeadingCommand(m_robotDrive));
       // Left bumper = Toggle fastmode
+      // Left trigger = Toggle fastermode
       // POV = Nudge
       // Right joystick = Move
       // Left joystick = Turn
@@ -135,12 +152,21 @@ boolean getFastMode() {
     return fastMode;
   }
 
-  double getRightX() {return m_driverController.getRightX();}
+  boolean getFasterMode() {
+    if (m_driverController.getLeftTriggerAxis() > OIConstants.kTriggerDeadband) {
+      fasterMode = true;
+    }
+    else fasterMode = false;
+    return fasterMode;
+  }
+  double getRightX() {
+    System.out.println("DELAY: " + m_delay.getDouble(0));
+    return m_driverController.getRightX();}
   double getLeftX() {return -m_driverController.getLeftX();}
   double getLeftY() {return -m_driverController.getLeftY();}
   double getPOV() {return m_driverController.getPOV();}
-  double getAuxRightY() {return m_auxController.getRightY();}
-  double getAuxLeftY() {return m_auxController.getLeftY();}
+  double getAuxRightY() {return Math.abs(m_auxController.getRightY()) > OIConstants.kDriveDeadband ? m_auxController.getRightY() : 0;}
+  double getAuxLeftY() {return Math.abs(m_auxController.getLeftY()) > OIConstants.kDriveDeadband ? m_auxController.getLeftY() : 0;}
   double getAuxPOV() {return m_auxController.getPOV();}
   double POVToWinchSpeed() {
     return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.kWinchDownPower : 0);

src/main/java/frc/robot/commands/ResetWinchCommand.java

@@ -1,7 +1,5 @@
 package frc.robot.commands;
 
-import java.util.function.Supplier;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ShooterSubsystem;
 

src/main/java/frc/robot/commands/ShootCommand.java

@@ -7,7 +7,7 @@ public ShootCommand (
         ShooterSubsystem shooterSubsystem
     ) {
         addCommands(
-            new ShooterCommand(shooterSubsystem, "both").withTimeout(3),
+            new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75),
             new ShooterCommand(shooterSubsystem, "all")
         );
     }

src/main/java/frc/robot/commands/SwerveDriveCommand.java

@@ -17,7 +17,7 @@ public class SwerveDriveCommand extends Command {
 
   public final SwerveSubsystem swerveSubsystem;
   public final Supplier<Double> xSpdFunction, ySpdFunction, turningSpdFunction, pov;
-  public Supplier<Boolean> fastModeFunction;
+  public Supplier<Boolean> fastModeFunction, fasterModeFunction;
   public final SlewRateLimiter xLimiter, yLimiter, turningLimiter;
   public boolean fieldOrientedFunction;
   private ShuffleboardTab m_tab;
@@ -30,6 +30,7 @@ public SwerveDriveCommand(
     Supplier<Double> turnSupplier,
     boolean fieldOriented,
     Supplier<Boolean> fastMode,
+    Supplier<Boolean> fasterMode,
     Supplier<Double> povSupplier
   ) {
     swerveSubsystem = subsystem;
@@ -38,6 +39,7 @@ public SwerveDriveCommand(
     turningSpdFunction = turnSupplier;
     fieldOrientedFunction = fieldOriented;
     fastModeFunction = fastMode;
+    fasterModeFunction = fasterMode;
     pov = povSupplier;
     xLimiter = new SlewRateLimiter(DriveConstants.kXSlewRateLimit);
     yLimiter = new SlewRateLimiter(DriveConstants.kYSlewRateLimit);
@@ -52,12 +54,13 @@ public SwerveDriveCommand(
   public void execute() {
     // Get inputs
     double xSpeed = 0, ySpeed = 0, turningSpeed = 0;
-    boolean fastMode = false;
+    boolean fastMode = false, fasterMode = false;
     if (pov.get() == -1) {
       xSpeed = xSpdFunction.get();
       ySpeed = ySpdFunction.get();
       turningSpeed = turningSpdFunction.get();
       fastMode = fastModeFunction.get();
+      fasterMode = fasterModeFunction.get();
     }
     else {
       xSpeed = pov.get() == 90 ? -DriveConstants.kNudgeSpeed : (pov.get() == 270 ? DriveConstants.kNudgeSpeed : 0);
@@ -70,8 +73,8 @@ public void execute() {
     turningSpeed = Math.abs(turningSpeed) > OIConstants.kDriveDeadband ? turningSpeed : 0;
 
     // Slew soup
-    double maxDriveSpeed = fastMode ? DriveConstants.kFastTeleMaxMetersPerSec : DriveConstants.kTeleMaxMetersPerSec;
-    double maxTurnSpeed = fastMode ? DriveConstants.kFastTeleMaxRadiansPerSec : DriveConstants.kTeleMaxRadiansPerSec;
+    double maxDriveSpeed = fasterMode ? DriveConstants.kFasterTeleMaxMetersPerSec : (fastMode ? DriveConstants.kFastTeleMaxMetersPerSec : DriveConstants.kTeleMaxMetersPerSec);
+    double maxTurnSpeed = fasterMode ? DriveConstants.kFasterTeleMaxRadiansPerSec : (fastMode ? DriveConstants.kFastTeleMaxRadiansPerSec : DriveConstants.kTeleMaxRadiansPerSec);
     xSpeed = xLimiter.calculate(xSpeed) * maxDriveSpeed;
     ySpeed = yLimiter.calculate(ySpeed) * maxDriveSpeed;
     turningSpeed = turningLimiter.calculate(turningSpeed) * maxTurnSpeed;

src/main/java/frc/robot/commands/TurnThetaCommand.java

@@ -41,7 +41,7 @@ public TurnThetaCommand(SwerveSubsystem subsystem, double turnDegrees) {
 
   @Override
   public void initialize(){
-    swerveSubsystem.zeroHeading();
+    swerveSubsystem.resetPose();
     initial_heading = swerveSubsystem.getHeading();
     isTurnFinished = false;
   }
@@ -51,6 +51,7 @@ public void execute() {
     //autoturny stuffs
     double theta = targetTheta - swerveSubsystem.getHeading();
     double speed = theta / 45;
+    System.out.println("DeltaTheta:" + theta);
 
     if(Math.abs(speed) < 0.005){ //TODO may need to adjust how sensitive it is
       isTurnFinished = true;
@@ -66,6 +67,7 @@ public void execute() {
     turningSpeed = MathUtil.clamp(speed, -1.0,1.0);
     xSpeed = 0;
     ySpeed = 0;
+    System.out.println("TurningSpeed:" + turningSpeed);
 
     // Death
     xSpeed = Math.abs(xSpeed) > OIConstants.kDriveDeadband ? xSpeed : 0;

src/main/java/frc/robot/commands/ZeroHeadingCommand.java

@@ -13,6 +13,6 @@ public ZeroHeadingCommand(SwerveSubsystem subsystem) {
 
   @Override
   public void execute() {
-    m_subsystem.zeroHeading();
+    m_subsystem.resetPose();
   }
 }

src/main/java/frc/robot/commands/auto/DriveAutoCommand.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.auto;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.SetPoseCommand;
+import frc.robot.commands.ShootCommand;
+import frc.robot.commands.SwerveDriveCommand;
+import frc.robot.commands.TurnThetaCommand;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class DriveAutoCommand extends SequentialCommandGroup {
+  public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) {
+    super(
+      new WaitCommand(delay),
+      new SetPoseCommand(swerve, startingOffset).withTimeout(0),
+      new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1))
+      .withTimeout(3),
+      new TurnThetaCommand(swerve, 0)
+    );
+  }
+
+  private static Supplier<Double> supplyDouble(double d) {return new Supplier<Double>() {@Override public Double get() {return d;}};}
+  private static Supplier<Boolean> supplyBoolean(boolean b) {return new Supplier<Boolean>() {@Override public Boolean get() {return b;}};}
+}

src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java

@@ -10,19 +10,23 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.SetPoseCommand;
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.SwerveDriveCommand;
+import frc.robot.commands.TurnThetaCommand;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
 
 public class ShootAndDriveAutoCommand extends SequentialCommandGroup {
-  public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset) {
+  public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) {
     super(
+      new WaitCommand(delay),
       new ShootCommand(shoot).withTimeout(4),
       new SetPoseCommand(swerve, startingOffset).withTimeout(0),
-      new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyDouble(0))
-      .withTimeout(3)
+      new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1))
+      .withTimeout(3),
+      new TurnThetaCommand(swerve, 0)
     );
   }