Merge pull request #14 from FairportRobotics/FRCCompIntakeMan

Finger lake changes

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "${wpiVersion}"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
 }
 
 java {

lib/RoboLib

@@ -1 +1 @@
-Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee
+Subproject commit eea76939a3cbb76aba47874adf15eaf49a8333de

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.measure.Angle;
 import java.security.PublicKey;
+import java.util.Queue;
 
 import com.ctre.phoenix6.signals.InvertedValue;
 
@@ -68,10 +69,10 @@ public final class Constants {
   }
   
   public static class IntakeConstants {
-    public static final int DEPLOY_MOTOR_ID = 12;
+    public static final int DEPLOY_MOTOR_ID = 17;
-    public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
+    public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this
 
-    public static final int INTAKE_MOTOR_ID = 1; 
+    public static final int INTAKE_MOTOR_ID = 0; 
     public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
   }
 
@@ -79,31 +80,33 @@ public final class Constants {
     public static final int LAUNCHER_MOTOR_ID = 2;
     public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
 
-    public static final int TURRET_MOTOR_ID = 1;
+    public static final int TURRET_MOTOR_ID = 0;
     public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
     public static final int TURRET_LIMIT_CHANNEL = 0;
     public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265);
     public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
     public static final double HOMING_SPEED = .35;
-    public static final double TURRET_GEAR_RATIO  = 1;
+    public static final double TURRET_GEAR_RATIO  = 0;
 
-    public static final int HOOD_SERVO_CHANNEL = 3;
+    public static final int HOOD_SERVO_CHANNEL = 9;
     public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
-    public static final int HOOD_ENCODER_ID = 4;
+    public static final int HOOD_ENCODER_ID = 0;
-    public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
+    public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5);
     public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
     public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
     public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0);
-    public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(20);
+    public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100);
-    public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(40);
+    public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200);
-    public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(60);
+    public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300);
+    public static final double DEFLECTOR_SERVO_RATIO = 1.0;
   }
   
   public static class HopperConstants {
-    public static final int KICKER_MOTOR_ID = 0;
+    public static final int KICKER_MOTOR_ID = 9;
-    public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
+    public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
 
-    public static final int SPINDEXER_MOTOR_ID = 1;
+    public static final int SPINDEXER_MOTOR_ID = 8;
-    public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
+    public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
+    public static final double SPINDEXER_MOTOR_SPEED = .6;
   }
 }

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

@@ -18,6 +18,7 @@ import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.ManualHopperCommand;
+import frc.robot.commands.ManualIntake;
 import frc.robot.commands.ManualShootCommand;
 import frc.robot.commands.SetDeflectorCommand;
 import frc.robot.subsystems.DriveSubsystem;
@@ -81,9 +82,15 @@ public class RobotContainer {
     m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
     m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
 
-    m_driverController.y().onTrue(intakeSubsystem.deployCommand());
+    /*m_driverController.y().onTrue(intakeSubsystem.deployCommand());
     m_driverController.a().onTrue(intakeSubsystem.retractCommand());
-    m_driverController.b().onTrue(intakeSubsystem.reverseCommand());
+    m_driverController.b().onTrue(intakeSubsystem.reverseCommand());*/
+
+    m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
+    m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
+    //m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand());
+
+    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
   }
 
   /**

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

@@ -4,6 +4,7 @@
 
 package frc.robot.commands;
 
+import frc.robot.Constants;
 import frc.robot.subsystems.HopperSubsystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,13 +23,13 @@ public class ManualHopperCommand extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_subsystem.spin(.5);
+    
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.5)
+    if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
     {
       m_subsystem.feed();
     }
@@ -36,6 +37,14 @@ public class ManualHopperCommand extends Command {
     {
       m_subsystem.stop();
     }
+    if(m_xboxController.getRightBumper())
+    {
+      m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED);
+    }
+    if(m_xboxController.getLeftBumper())
+    {
+      m_subsystem.spin(0);
+    }
   }
 
   // Called once the command ends or is interrupted.

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

@@ -0,0 +1,62 @@
+// 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;
+
+import frc.robot.subsystems.IntakeSubsystem;
+
+import javax.lang.model.util.ElementScanner14;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class ManualIntake extends Command {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final IntakeSubsystem m_subsystem;
+  private final XboxController m_XboxController;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
+    m_subsystem = subsystem;
+    m_XboxController = xboxController;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(m_XboxController.getLeftBumper()==true)
+    {
+      m_subsystem.extend();
+    }
+    else if(m_XboxController.getRightBumper()==true)
+    {
+      m_subsystem.retract();
+    }
+    else
+    {
+      m_subsystem.neutral();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -6,6 +6,7 @@ package frc.robot.commands;
 
 import frc.robot.subsystems.TurretSubsystem;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** An example command that uses an example subsystem. */
@@ -23,13 +24,25 @@ public class ManualShootCommand extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(m_controller.getLeftTriggerAxis()>.5 || m_controller.getRightTriggerAxis()>.5)
+    if(m_controller.getRightTriggerAxis()>.8)
     {
       m_subsystem.setLauncher(1);
+      m_controller.setRumble(RumbleType.kBothRumble, 1);
+    }
+    else if(m_controller.getRightTriggerAxis()>.5)
+    {
+      m_subsystem.setLauncher(.7);
+      m_controller.setRumble(RumbleType.kBothRumble, .67);
+    }
+    else if(m_controller.getRightTriggerAxis()>.2)
+    {
+      m_subsystem.setLauncher(.5);
+      m_controller.setRumble(RumbleType.kBothRumble, .33);
     }
     else
     {
       m_subsystem.setLauncher(0);
+      m_controller.setRumble(RumbleType.kBothRumble, 0);
     }
   }
 

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

@@ -128,7 +128,7 @@ public class DriveSubsystem extends TestableSubsystem {
 
             @Override
             public void run() {
-                driveSystem.setChassisSpeedsFromJoystickRobotRelative(
+                driveSystem.setChassisSpeedsFromJoystickFieldRelative(
                         -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())),
                         -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
                         -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
@@ -197,7 +197,6 @@ public class DriveSubsystem extends TestableSubsystem {
         }
 
         assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();
-
     }
 
 }

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

@@ -69,12 +69,12 @@ public class IntakeSubsystem extends TestableSubsystem {
 
   public Command deployCommand()
   {
-    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(50), () -> setSpeed(0), this));
+    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(.5), () -> setSpeed(0), this));
   }
 
   public Command reverseCommand()
   {
-    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-50), () -> setSpeed(0), this));
+    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-.5), () -> setSpeed(0), this));
   }
 
   public Command retractCommand()
@@ -82,13 +82,18 @@ public class IntakeSubsystem extends TestableSubsystem {
     return this.runOnce(() -> setSpeed(0)).andThen(new FunctionalCommand(this::retract, this::retract, interrupted -> neutral(), this::isRetracted, this));
   }
 
-  @PostTest(name = "A friendly name", enabled = true)
+  public Command startSpeedCommand()
-  public void myFailingPostTest(){
+  {
-    assertThat(true).isFalse();
+    return this.runOnce(() -> setSpeed(-.5));
   }
 
-  @PostTest(enabled = true)
+  public Command killSpeedCommand()
-  public void myPassingPostTest(){
+  {
-    assertThat("Hello World").endWith("World");
+    return this.runOnce(() -> setSpeed(0));
+  }
+
+  public Command revSpeedCommand()
+  {
+    return this.runOnce(() -> setSpeed(.5));
   }
 }

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

@@ -33,11 +33,9 @@ public class TurretSubsystem extends TestableSubsystem {
   //private TalonFX turretMotor;
   private SparkMax launcherMotor;
   private Servo hood;
-  private CANcoder hoodEncoder;
   private Angle limitPos;
   private Angle limitNeg;
   private boolean turretGoingPos;
-  private Angle targElev;
 
   public TurretSubsystem() {
     turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
@@ -50,42 +48,42 @@ public class TurretSubsystem extends TestableSubsystem {
     config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
 
     hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
-    hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID);
     currentState = TurretState.INIT;
     limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
     limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
     turretGoingPos = false;
-    targElev = edu.wpi.first.units.Units.Degrees.of(45);
   }
 
   public void setLauncher(double speed) {launcherMotor.set(speed);}
 
   public void setTargetElevation(Angle elev)
   {
-    if(elev.gt(Constants.ShooterConstants.LIMIT_ELEVATION_NEG) && elev.lt(Constants.ShooterConstants.LIMIT_ELEVATION_POS))
+    if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){
-    {
+      elev=Units.Degrees.of(300).minus(elev);
-      targElev=elev;
     }
+    elev=elev.div(1.5);
+    elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO);
+    hood.setAngle(elev.in(Units.Degrees));
   }
 
   public void startHoming()
   {
     //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
-    currentState = TurretState.HOMING;
+    //currentState = TurretState.HOMING;
   }
 
   public void startRotate(double speed)
   {
-    turretGoingPos=speed>0;
+    /*turretGoingPos=speed>0;
 
     if(!passedLimit())
     {
-      //turretMotor.set(speed);
+      turretMotor.set(speed);
     }
     else
     {
-      //turretMotor.set(0);
+      turretMotor.set(0);
-    }
+    }*/
   }
 
   public boolean passedLimit(){
@@ -126,31 +124,11 @@ public class TurretSubsystem extends TestableSubsystem {
 
   private void periodicManual()
   {
-    if(passedLimit())
+    //If the turret move, it will be a uh-oh, so DONT MOVE IT
+    
+    /*if(passedLimit())
     {
       //turretMotor.set(0);
-    }
+    }*/
-
-    int k = 1;
-    if(Constants.ShooterConstants.HOOD_SERVO_INVERTED) {
-      k=-1;
-    }
-
-    Angle curElev = hoodEncoder.getAbsolutePosition().getValue();
-    if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees))
-    {
-      if(targElev.gt(curElev))
-      {
-        hood.set(k * 0.5);
-      }
-      else
-      {
-        hood.set(k * -0.5);
-      }
-    }
-    else
-    {
-      hood.set(0);
-    }
   }
 }