Pre-Manor Comp Code

.gitignore

@@ -22,3 +22,4 @@
 # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
 hs_err_pid*
 replay_pid*
+.DS_Store

2024-Crescendo/src/main/java/frc/robot/Constants.java

@@ -14,8 +14,8 @@
  */
 public final class Constants {
   public static class RobotConstants {
-    public static final double minPnuematicsPressure = 80.0;
-    public static final double maxPnuematicsPressure = 120.0;
+    public static final double minPnuematicsPressure = 80.0;  // default 80.0
+    public static final double maxPnuematicsPressure = 120.0; // default 120.0
 
     public static final int periodicTicksPerSecond = 50;
     public static final int pnuematicReportingFreq = 1;
@@ -36,19 +36,16 @@ public static class DrivetrainConstants {
   }
 
   public static class DriverConstants {
-    public static final int kLeftDriveJoystick = 0;
-    public static final int kRightDriveJoystick = 1;
+    public static final int kDriveJoystick = 0;
 
     public static final double kTurnDivider = 2.0;
     public static final double kLowGearDivider = 2.0;
 
     // Left Joystick
     public static final int kLowGear = 1; // Joystick Trigger
-    public static final int kReverseMode = 9; // Joystick button 9
+    public static final int kReverseMode = 11; // Joystick button 11
     public static final int kForwardMode = 7; // Joystick button 7
-
-    // Right Joystick
-    public static final int kDriveStraight = 1; // Joystick Trigger
+
   }
 
   public static class OperatorConstants {
@@ -69,8 +66,8 @@ public static class OperatorConstants {
     public static final int kSpitOut = 10;  // right joystick down
 
     public static final double kGooseAngleIncrement = 1.0;
-    public static final double kGooseAngleMax = 130.0;
-    public static final double kGooseAngleMin = -10.0;
+    public static final double kGooseAngleMax = 120.0;
+    public static final double kGooseAngleMin = 0.0;
   }
 
   public static class ClimbConstants {
@@ -100,6 +97,7 @@ public static class NoteHandlingConstants {
 
   public static class GooseRotationConstants {
     public static final int kRotateMotorCANID = 40;
+    public static final int kSmartMotionSlot  = 0;
 
     // TODO: Need to tune
     // SparkMax PID coefficients for the rotation controller.
@@ -112,21 +110,28 @@ public static class GooseRotationConstants {
     public static final double kMinOutput = -1.0;
     public static final double kMaxOutput = 1.0;
 
+    public static final double kMaxVelocity     = 1500.0;
+    public static final double kMinVelocity     = 0.0;
+    public static final double kMaxAcceleration = 500.0;
+    public static final double kMaxError        = 0.1;
+
+    public static final boolean kUseSmartMotion = false;
+
     // Starting in a non-end positon, there will be a switch at one or both of the end positions.
     // When the switch(s) are hit the software will reset to the known position.
     // Currently we are presuming there will be a thing to set the arm at the start of the game.
     // TODO: The next 7 values are guesses and need to be measured.
     public static final double kStartingAngle         = 30.0;
 
-    public static final double kBottomRawEncoderValue = 0.680;
-    public static final double kBottomAngle           = -10.0;
+    public static final double kBottomRawEncoderValue = 0.523;
+    public static final double kBottomAngle           = 0.0;
 
-    public static final double kTopRawEncoderValue    = 0.386;
-    public static final double kTopAngle              = 115.0; 
+    public static final double kTopRawEncoderValue    = 0.205;
+    public static final double kTopAngle              = 115.0;
 
     public static final double kScoreAngle            = 130.0;
     public static final double kMiddleAngle           = 40.0;
-    public static final double kIntakeAngle           = -5.7;
+    public static final double kIntakeAngle           = 0.0;
 
     // This defines the gear ratio between the motor and output shafts. Divide
     // the motor rotation count by this number to determine the output shaft

2024-Crescendo/src/main/java/frc/robot/RobotContainer.java

@@ -11,7 +11,6 @@
 import frc.robot.commands.ClimbCommand;
 import frc.robot.commands.NoteHandlingSpeedCommand;
 import frc.robot.commands.RotateCommand;
-import frc.robot.commands.SetDriveStraightCommand;
 import frc.robot.commands.SetGearCommand;
 import frc.robot.commands.SetModeCommand;
 import frc.robot.subsystems.DriveSubsystem;
@@ -38,7 +37,6 @@ public class RobotContainer {
 
   // Create SmartDashboard chooser for autonomous routines
   private final SendableChooser<Command> m_autoChooser = new SendableChooser<>();
-  private final SendableChooser<Boolean> m_driveTypeChooser = new SendableChooser<>();
 
   // The robot's subsystems and commands are defined here...
   private final PneumaticHub   m_pnuematicHub   = new PneumaticHub();
@@ -49,20 +47,16 @@ public class RobotContainer {
 
   // Joysticks
   private final XboxController m_operatorController = new XboxController(OperatorConstants.kOperatorController);
-  private final Joystick m_driverJoystickLeft = new Joystick(DriverConstants.kLeftDriveJoystick);
-  private final Joystick m_driverJoystickRight = new Joystick(DriverConstants.kRightDriveJoystick);
+  private final Joystick m_driverJoystick = new Joystick(DriverConstants.kDriveJoystick);
 
   // Driver Buttons
   public final JoystickButton m_lowGearButton =
-    new JoystickButton(m_driverJoystickLeft, DriverConstants.kLowGear);
+    new JoystickButton(m_driverJoystick, DriverConstants.kLowGear);
 
   public final JoystickButton m_forwardModeButton = 
-    new JoystickButton(m_driverJoystickLeft, DriverConstants.kForwardMode);
+    new JoystickButton(m_driverJoystick, DriverConstants.kForwardMode);
   public final JoystickButton m_reverseModeButton = 
-    new JoystickButton(m_driverJoystickLeft, DriverConstants.kReverseMode);
-
-  public final JoystickButton m_driveStraightButton = 
-    new JoystickButton(m_driverJoystickRight, DriverConstants.kDriveStraight);
+    new JoystickButton(m_driverJoystick, DriverConstants.kReverseMode);
 
   // Pnuematics Climb Buttons
   private final JoystickButton m_climbUpButton =
@@ -112,12 +106,6 @@ public void reportStatus() {
       SmartDashboard.putBoolean("Compressor Running", m_pnuematicHub.getCompressor());
     }
 
-    if(m_tickCount % (RobotConstants.periodicTicksPerSecond/RobotConstants.imuReportingFreq) == 0)
-    {
-      // Report IMU state.
-      SmartDashboard.putNumber("Heading", m_driveSubsystem.getHeading());
-    }
-
     if(m_tickCount % (RobotConstants.periodicTicksPerSecond/RobotConstants.gooseReportingFreq) == 0)
     {
       // Report Goose ARM state.
@@ -139,28 +127,18 @@ private void configureDriverStationControls()
     m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDrive4Sec(m_driveSubsystem));
     m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDrive6Sec(m_driveSubsystem));
     SmartDashboard.putData(m_autoChooser);
-
-    // Drive control option selector
-    m_driveTypeChooser.setDefaultOption("Arcade Drive", true);
-    m_driveTypeChooser.addOption("Tank Drive", false);
-    SmartDashboard.putData(m_driveTypeChooser);
   }
 
   private void configureBindings() 
   {
     // Drive Controllers. We set arcade drive as the default here but may change this
     // during teleopInit depending upon the value of a dashboard chooser.
-    m_driveSubsystem.initDefaultCommand(m_driverJoystickLeft, m_driverJoystickRight, true);
-
     m_lowGearButton.onTrue(new SetGearCommand(m_driveSubsystem, true));
     m_lowGearButton.onFalse(new SetGearCommand(m_driveSubsystem, false));
 
     m_forwardModeButton.onTrue(new SetModeCommand(m_driveSubsystem, false));
     m_reverseModeButton.onTrue(new SetModeCommand(m_driveSubsystem, true));
 
-    m_driveStraightButton.onTrue(new SetDriveStraightCommand(m_driveSubsystem, true));
-    m_driveStraightButton.onFalse(new SetDriveStraightCommand(m_driveSubsystem, false));
-
     // Climb Buttons
     m_climbUpButton.onTrue(new ClimbCommand(m_climbSubsystem, ClimbSubsystem.State.LIFTED));
     m_climbDownButton.onTrue(new ClimbCommand(m_climbSubsystem, ClimbSubsystem.State.GROUNDED));
@@ -206,11 +184,9 @@ public Command getAutonomousCommand() {
     return m_autoChooser.getSelected();
   }
 
-  // Set the driver's choice of control mode based on the selection provided
-  // in a dashboard control.
+  // Set drive command to arcade
   public void setDriveType()
   {
-    Boolean bArcade = m_driveTypeChooser.getSelected();
-    m_driveSubsystem.initDefaultCommand(m_driverJoystickLeft, m_driverJoystickRight, bArcade);
+    m_driveSubsystem.initDefaultCommand(m_driverJoystick);
   }
 }

2024-Crescendo/src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -5,29 +5,26 @@
 package frc.robot.subsystems;
 
 import frc.robot.Constants.*;
-import frc.robot.commands.TankDriveCommand;
 import frc.robot.commands.ArcadeDriveCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.SPI;
 
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.kauailabs.navx.frc.AHRS;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 public class DriveSubsystem extends SubsystemBase
 {
   private final TalonFX m_leftFrontMotor  = new TalonFX(DrivetrainConstants.kLeftFrontMotorCANID);
   private final TalonFX m_leftBackMotor   = new TalonFX(DrivetrainConstants.kLeftBackMotorCANID);
   private final TalonFX m_rightFrontMotor = new TalonFX(DrivetrainConstants.kRightFrontMotorCANID);
   private final TalonFX m_rightBackMotor  = new TalonFX(DrivetrainConstants.kRightBackMotorCANID);
-  private final AHRS m_NavX               = new AHRS(SPI.Port.kMXP);
   private DifferentialDrive m_Drivetrain;
   private double m_leftSpeed  = 0.0;
   private double m_rightSpeed = 0.0;
-  private double m_speedDivider = 1.0;
+  private double m_speedDivider = 2.5; // Default 1.0
   private double m_modeMultiplier = 1.0;
   private boolean m_driveStraight = false;
 
@@ -42,28 +39,24 @@ public DriveSubsystem()
     m_rightFrontMotor.setInverted(true);
     m_rightBackMotor.setInverted(true);
 
+    // Set all motors to brake mode for safety. In the default, coast mode,
+    // the robot takes very much longer to stop if the joystick is released or
+    // and emergency stop occurs. In this mode, it stops very quickly.
+    m_leftFrontMotor.setNeutralMode(NeutralModeValue.Brake);
+    m_leftBackMotor.setNeutralMode(NeutralModeValue.Brake);
+    m_rightFrontMotor.setNeutralMode(NeutralModeValue.Brake);
+    m_rightBackMotor.setNeutralMode(NeutralModeValue.Brake);
+
     /* Set back motors to follow the front motors. */
     m_leftBackMotor.setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
     m_rightBackMotor.setControl(new Follower(m_rightFrontMotor.getDeviceID(), false));
 
     m_Drivetrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
-
-
-    // Set gyro 0 angle to point forward.
-    m_NavX.reset();
-    m_NavX.zeroYaw();
   }
 
-  public void initDefaultCommand(Joystick leftJoystick, Joystick rightJoystick, Boolean bArcadeDrive)
+  public void initDefaultCommand(Joystick leftJoystick)
   {
-    if (bArcadeDrive)
-    {
-      setDefaultCommand(new ArcadeDriveCommand(this, leftJoystick));
-    }
-    else
-    {
-      setDefaultCommand(new TankDriveCommand(this, leftJoystick, rightJoystick));
-    }
+    setDefaultCommand(new ArcadeDriveCommand(this, leftJoystick));
   }
 
   // Sets left and right motors to set speeds to support tank drive models.
@@ -118,11 +111,6 @@ public double getSpeed(boolean bLeft)
     }
   }
 
-  public double getHeading()
-  {
-    return (double)m_NavX.getYaw();
-  }
-
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

2024-Crescendo/src/main/java/frc/robot/subsystems/GooseRotationSubsystem.java

@@ -69,6 +69,14 @@ public GooseRotationSubsystem()
     m_pidController.setFF(m_kFF);
     m_pidController.setOutputRange(m_kMinOutput, m_kMaxOutput);
 
+    if (GooseRotationConstants.kUseSmartMotion)
+    {
+      m_pidController.setSmartMotionMaxVelocity(GooseRotationConstants.kMaxVelocity, GooseRotationConstants.kSmartMotionSlot);
+      m_pidController.setSmartMotionMinOutputVelocity(GooseRotationConstants.kMinVelocity, GooseRotationConstants.kSmartMotionSlot);
+      m_pidController.setSmartMotionMaxAccel(GooseRotationConstants.kMaxAcceleration, GooseRotationConstants.kSmartMotionSlot);
+      m_pidController.setSmartMotionAllowedClosedLoopError(GooseRotationConstants.kMaxError, GooseRotationConstants.kSmartMotionSlot);
+    }
+
     // display PID coefficients on SmartDashboard
     SmartDashboard.putNumber("P Gain", m_kP);
     SmartDashboard.putNumber("I Gain", m_kI);
@@ -104,7 +112,14 @@ public void setSetPointAngle(double angleDegrees)
     double setpoint = getMotorPositionFromAngle(angleDegrees);
     SmartDashboard.putNumber("Set Degress", m_angleSet);
     SmartDashboard.putNumber("Set Reference", setpoint);
-    m_pidController.setReference(setpoint, CANSparkBase.ControlType.kPosition);
+    if(GooseRotationConstants.kUseSmartMotion)
+    {
+      m_pidController.setReference(setpoint, CANSparkBase.ControlType.kSmartMotion);
+    }
+    else
+    {
+      m_pidController.setReference(setpoint, CANSparkBase.ControlType.kPosition);
+    }
   }
 
   public double getSetPointAngle()

2025-Reefscape/.gradle/buildOutputCleanup/cache.properties

@@ -0,0 +1,2 @@
+#Tue Jan 13 18:35:10 CST 2026
+gradle.version=8.11