small cleanup as comp is getting closer

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

@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.utils.UtilFuncs;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
@@ -65,8 +64,8 @@ public static class Speeds {
     public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3;
     public static final double ELEVATOR_MAX_SPEED = 0.2;
 
-    public static final double INTAKE_FEED_MAX_SPEED = 0.25; // TODO: Get this
-    public static final double OUTTAKE_FEED_MAX_SPEED = -0.4;
+    public static final double INTAKE_FEED_SPEED = 0.25; // TODO: Get this
+    public static final double OUTTAKE_FEED_SPEED = -0.4;
 
     public static final double INTAKE_ACTUATE_MAX_SPEED = 0.4;
   }
@@ -80,13 +79,7 @@ public static class Physical {
     public static final double SWERVE_DRIVE_WHEEL_RADIUS = 0.05;
     public static final double SWERVE_DRIVE_WHEEL_CIRCUMFERENCE = 2 * Math.PI * SWERVE_DRIVE_WHEEL_RADIUS;
 
-    public static final double SHOOTER_FLYWHEEL_GEAR_RATIO = 1.45; // TODO: FIND THIS
     public static final double SHOOTER_ANGLE_GEAR_RATIO = 112;
-    public static final double SHOOTER_FLYWHEEL_RADIUS = 1; // TODO: FIND RADIUS
-    public static final double SHOOTER_FLYWHEEL_CIRCUMFERENCE = 2 * Math.PI * SHOOTER_FLYWHEEL_RADIUS;
-
-    public static final double ELEVATOR_GEAR_RATIO = 100; // TODO: fixed but they might change it
-
     public static final double SHOOTER_HEIGHT_STOWED = 0.2; // TODO: Get this value
 
     public static final SwerveDriveKinematics SWERVE_KINEMATICS = new SwerveDriveKinematics(
@@ -100,10 +93,12 @@ public static class Encoders {
   }
 
   public static class FeedForward {
-    public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constants
+    public static final double ELEVATOR_KG = 0.0;
 
     public static final double MODULE_DRIVE_KS = 0.32;
     public static final double MODULE_DRIVE_KV = 2.15;
+
+    public static final double SHOOTER_ANGLE_KG = 0.001;
   }
 
   public static class PID {
@@ -112,6 +107,10 @@ public static class PID {
     public static final double MODULE_DRIVE_KP = 0.05;
     public static final double MODULE_ROTATION_KP = 0.009;
 
+    public static final double SHOOTER_ANGLE_KP = 0.08;
+
+    public static final double INTAKE_ACTUATE_KP = 0.025;
+
     // these are all the same, so the two constants above are used instead
     // public static final double FRONT_LEFT_DRIVE_KP = 0.05;
     // public static final double FRONT_RIGHT_DRIVE_KP = 0.05;
@@ -136,12 +135,12 @@ public static class PID {
 
   public static class Offsets {
     // these aren't used anymore because cancoders can be zeroed in phoenix tuner
-    public static final double ENCODER_FRONT_LEFT = -93;
-    public static final double ENCODER_FRONT_RIGHT = -58;
-    public static final double ENCODER_BACK_RIGHT = 10;
-    public static final double ENCODER_BACK_LEFT = 43;
+    // public static final double ENCODER_FRONT_LEFT = -93;
+    // public static final double ENCODER_FRONT_RIGHT = -58;
+    // public static final double ENCODER_BACK_RIGHT = 10;
+    // public static final double ENCODER_BACK_LEFT = 43;
 
-    public static final double APRILTAG_SPEAKER_OFFSET = 0.565; // <- Below, but in Meters
+    // public static final double APRILTAG_SPEAKER_OFFSET = 0.565; // <- Below, but in Meters
     // 200(approx height of spk opening) - 132(Height of AprTag) + 11.5(Center of
     // AprTag) <- CM
   }

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

@@ -191,13 +191,6 @@ private void configureBindings() {
     // );
 
 
-    // _operatorController.circle().whileTrue(
-    //   Commands.run(() -> _intakeSubsystem.actuate(-0.3), _intakeSubsystem).handleInterrupt(() -> _intakeSubsystem.actuate(0))
-    // );
-    // _operatorController.cross().whileTrue(
-    //   Commands.run(() -> _intakeSubsystem.actuate(0.3), _intakeSubsystem).handleInterrupt(() -> _intakeSubsystem.actuate(0))
-    // );
-
     // _operatorController.circle().whileTrue(new FeedIntake(_intakeSubsystem, ActuatorState.NONE, FeedMode.INTAKE));
     // _operatorController.cross().whileTrue(new FeedIntake(_intakeSubsystem, ActuatorState.NONE, FeedMode.OUTTAKE));
 

src/main/java/frc/robot/commands/leds/DefaultLED.java

@@ -3,7 +3,6 @@
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
 import frc.robot.Constants.LEDColors;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.utils.UtilFuncs;

src/main/java/frc/robot/commands/shooter/AutoAim.java

@@ -26,9 +26,6 @@
 public class AutoAim extends Command {
   private final ShooterSubsystem _shooter;
   private final SwerveDriveSubsystem _swerve;
-  private final VisionSubsystem _vision;
-  private final IntakeSubsystem _intake;
-  private final ElevatorSubsystem _elevator;
   private final LEDSubsystem _leds;
 
   private final DoubleSupplier _xSpeed;
@@ -43,8 +40,13 @@ public class AutoAim extends Command {
       Constants.PID.SWERVE_HEADING_KD);
 
   /** Creates a new AutoAim. */
-  public AutoAim(ShooterSubsystem shooter, LEDSubsystem leds, SwerveDriveSubsystem swerve,
-      DoubleSupplier xSpeed, DoubleSupplier ySpeed, VisionSubsystem vision, IntakeSubsystem intake, ElevatorSubsystem elevator) {
+  public AutoAim(
+    ShooterSubsystem shooter, 
+    LEDSubsystem leds, 
+    SwerveDriveSubsystem swerve,
+    DoubleSupplier xSpeed, 
+    DoubleSupplier ySpeed
+  ) {
     // Use addRequirements() here to declare subsystem dependencies.
     _leds = leds;
     _shooter = shooter;
@@ -58,16 +60,13 @@ public AutoAim(ShooterSubsystem shooter, LEDSubsystem leds, SwerveDriveSubsystem
     _headingController.setTolerance(2);
     _headingController.enableContinuousInput(-180, 180);
 
-    _vision = vision;
-    _intake = intake;
-    _elevator = elevator;
 
     addRequirements(_shooter, _swerve, _leds);
   }
 
   /** Creates an auton AutoAim that ends when it reaches the first setpoints. */
   public AutoAim(LEDSubsystem leds, ShooterSubsystem shooter, SwerveDriveSubsystem swerve, VisionSubsystem vision, IntakeSubsystem intake, ElevatorSubsystem elevator) {
-    this(shooter, leds, swerve, () -> 0, () -> 0, vision, intake, elevator);
+    this(shooter, leds, swerve, () -> 0, () -> 0);
 
     _runOnce = true;
   }
@@ -76,31 +75,21 @@ public AutoAim(LEDSubsystem leds, ShooterSubsystem shooter, SwerveDriveSubsystem
   @Override
   public void initialize() {
     _reachedSwerveHeading = false;
-    _reachedShooterAngle = false;
+    _reachedShooterAngle = true; // FOR NOW
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     double currentSwerveHeading = _swerve.getHeading().getDegrees();
     double desiredSwerveHeading = _swerve.speakerAngles()[0];
-    int speakerAprilTag = UtilFuncs.GetAlliance() == Alliance.Red ? 4 : 7;
 
     double rotationVelocity = MathUtil.clamp(
         _headingController.calculate(currentSwerveHeading, desiredSwerveHeading),
         -Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED * 2,
         Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED * 2);
 
     _reachedSwerveHeading = _headingController.atSetpoint();
-
-    if (_vision.isApriltagVisible(speakerAprilTag)){
-      _shooter.setAngle(_vision.tagAngleOffsets(speakerAprilTag)[1]); // Might need an angle offset prob better without limelight
-    }
-    else{
-      _shooter.setAngle(_swerve.speakerAngles()[1]);
-    }
-
-    _intake.setAngle(_shooter.getAngle());
 
     if (_reachedSwerveHeading)
       rotationVelocity = 0; // to prevent oscillation

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -4,7 +4,6 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkBase.SoftLimitDirection;
 
 import edu.wpi.first.math.MathUtil;
@@ -91,25 +90,22 @@ public double getElevatorHeight() {
    * Drives the elevator at a desired percent output (feedforward is included).
    */
   public void driveElevator(double speed) {
+    double ff = 0;
 
-    // double out = 0;
+    if (_usingClimberFeed)
+      ff = _climbFeed.calculate(0);
+    else {
+      ff = _elevatorFeed.calculate(0);
+    }
 
-    // if (_usingClimberFeed)
-    //   out = _climbFeed.calculate(0);
-    // else {
-    //   out = _elevatorFeed.calculate(0);
-    // }
+    ff = UtilFuncs.FromVolts(ff);
 
-    _leftMotor.set(speed);
+    _leftMotor.set(ff + speed);
   }
 
   /** Stops elevator movement. */
   public void stopElevator() {
     // System.out.println("Stopped");
     driveElevator(0);
   }
-
-  public void changeElevatorFeed() {
-    _usingClimberFeed = !_usingClimberFeed;
-  }
 }

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -15,14 +15,15 @@
 import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Constants.PID;
 import frc.robot.utils.configs.NeoConfig;
 
 /**
  * @author Peter Gutkovich
  */
 public class IntakeSubsystem extends SubsystemBase {
   private final CANSparkMax _feedMotor, _actuatorMotor;
-  private final PIDController _actuatorController = new PIDController(0.025, 0, 0);
+  private final PIDController _actuatorController = new PIDController(PID.INTAKE_ACTUATE_KP, 0, 0);
   // private final ProfiledPIDController _actuatorController = new ProfiledPIDController(
   //   0.05,
   //   0,
@@ -70,6 +71,8 @@ public IntakeSubsystem() {
    * 
    * @return True if the intake is moving against a note, else False.
    */
+
+  // TODO: why not working?
   public boolean noteSafety() {
     if (Math.abs(_feedMotor.get()) > 0 && Math.abs(_feedEncoder.getVelocity()) < 2) {
       return true;
@@ -92,11 +95,6 @@ public double getActuator() {
     return _actuatorEncoder.getPosition();
   }
 
-  // FOR TESTING ONLY
-  public void actuate(double speed) {
-    _actuatorMotor.set(speed);
-  }
-
   /**
    * Set the actuator's state. MUST BE CALLED REPEATEDLY.
    *
@@ -134,21 +132,16 @@ public void actuate(ActuatorState actuatorState) {
     _actuatorMotor.set(out);
   }
 
-  public void setAngle(double angle){
-    double out = 0;
+  // public void setAngle(double angle){
+  //   double out = 0;
 
-     out = MathUtil.clamp(
-          _actuatorController.calculate(getActuator(), angle),
-          -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED,
-          Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED);
+  //    out = MathUtil.clamp(
+  //         _actuatorController.calculate(getActuator(), angle),
+  //         -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED,
+  //         Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED);
 
-    _actuatorMotor.set(out);
-  }
-
-  // for testing
-  public void feed(double s) {
-    _feedMotor.set(s);
-  }
+  //   _actuatorMotor.set(out);
+  // }
 
   /**
    * Feed in/out of the intake.
@@ -159,11 +152,11 @@ public void feed(double s) {
   public void feed(FeedMode feedMode) {
     switch (feedMode) {
       case INTAKE :
-        _feedMotor.set(Constants.Speeds.INTAKE_FEED_MAX_SPEED);
+        _feedMotor.set(Constants.Speeds.INTAKE_FEED_SPEED);
         break;
 
       case OUTTAKE :
-        _feedMotor.set(Constants.Speeds.OUTTAKE_FEED_MAX_SPEED);
+        _feedMotor.set(Constants.Speeds.OUTTAKE_FEED_SPEED);
         break;
 
       case NONE :

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -12,6 +12,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Constants.FeedForward;
+import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
 import frc.robot.utils.UtilFuncs;
 import frc.robot.utils.configs.NeoConfig;
@@ -20,6 +22,7 @@
  * @author Elvis Osmanov
  * @author Peleh Liu
  * @author Cherine Soewingjo
+ * @author Peter Gutkovich
  */
 public class ShooterSubsystem extends SubsystemBase {
   private final CANSparkMax _leftMotor = new CANSparkMax(Constants.CAN.SHOOTER_LEFT, MotorType.kBrushless);
@@ -29,9 +32,10 @@ public class ShooterSubsystem extends SubsystemBase {
   private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder();
   private final RelativeEncoder _angleEncoder = _angleMotor.getEncoder();
 
-  private final ArmFeedforward _angleFeed = new ArmFeedforward(0.001, 0, 0); // nothing for now
-  private final PIDController _angleController = new PIDController(0.08, 0, 0);
+  private final ArmFeedforward _angleFeed = new ArmFeedforward(FeedForward.SHOOTER_ANGLE_KG, 0, 0);
+  private final PIDController _angleController = new PIDController(PID.SHOOTER_ANGLE_KP, 0, 0);
 
+  /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */
   public enum ShooterState {
     SHOOT,
     AMP,
@@ -41,8 +45,6 @@ public enum ShooterState {
   /** Creates a new ShooterSubsystem. */
   public ShooterSubsystem() {
     NeoConfig.configureNeo(_leftMotor, true);
-    // NeoConfig.configureNeo(_rightMotor, false);
-
     NeoConfig.configureFollowerNeo(_rightMotor, _leftMotor, true);
 
     NeoConfig.configureNeo(_angleMotor, true);
@@ -100,29 +102,7 @@ public void stopAngle() {
     driveAngle(0);
   }
 
-  // /** Get the velocity of the back wheel (left side) in m/s. */
-  // public double getVelocity() {
-  //   double neo_rps = _leftEncoder.getVelocity() / 60;
-
-  //   double number = (neo_rps / Constants.Physical.SHOOTER_GEAR_RATIO)
-  //       * Constants.Physical.SHOOTER_FLYWHEEL_CIRCUMFERENCE;
-
-  //   if (number < 0) {
-  //     number = 0;
-  //   }
-
-  //   return number;
-  // }
-
-  // /** Set the velocity of the back wheels in m/s. */
-  // public void setVelocity(double velocity) {
-  //   double flywheel_output = (velocity / Constants.Speeds.SHOOTER_MAX_SPEED); // FEEDFORWARD (main output)
-  //   double flywheel_pid = _shooterController.calculate(getVelocity(), velocity); // PID for distrubances
-
-  //   // a similar controller setup can be found in SwerveModule
-  //   _leftMotor.set(flywheel_output + flywheel_pid);
-  // }
-
+  /** Sets the state of the shooter. */
   public void setShooterState(ShooterState state) {
     switch (state) {
       case SHOOT:
@@ -145,12 +125,10 @@ public void setShooterState(ShooterState state) {
   /** Spins the shooter at the specified percent output. */
   public void spinShooter(double speed) {
     _leftMotor.set(speed);
-    // _rightMotor.set(speed); 
   }
 
   /** Stops spinning the shooter. */
   public void stopShooter() {
-    _leftMotor.set(0);
-    // _rightMotor.set(0);
+    spinShooter(0);
   }
 }

src/main/java/frc/robot/utils/SwerveModule.java

@@ -161,7 +161,6 @@ public void setState(SwerveModuleState state) {
 
     rotate(rotation_pid);
     drive(drive_feedforward + drive_pid);
-    // drive(0.08);
   }
 
   /**