Proto-bot Trial Run (#57)

* Update Constants.java

* DGRFVSRTDGFCEDGRFTCEXVDGRFTCXEZVSD5GYRFTCXZE

* auto did not work tonight :(

---------

Co-authored-by: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com>

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

@@ -206,7 +206,12 @@ public static class constAlgaeIntake {
 
   public static class constCoralOuttake {
     public static final double CORAL_OUTTAKE_SPEED = 0.3;
-    public static final Distance REQUIRED_CORAL_DISTANCE = Units.Inches.of(1);
+    public static final Distance REQUIRED_CORAL_DISTANCE = Units.Inches.of(2);
+
+    public static TalonFXConfiguration CORAL_OUTTAKE_CONFIG = new TalonFXConfiguration();
+    static {
+      CORAL_OUTTAKE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    }
   }
 
   public static class consClimber {
@@ -218,11 +223,9 @@ public static class constElevator {
     static {
       ELEVATOR_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
       ELEVATOR_CONFIG.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-      ELEVATOR_CONFIG.MotorOutput.PeakForwardDutyCycle = 0.5;
-      ELEVATOR_CONFIG.MotorOutput.PeakReverseDutyCycle = -0.5;
 
       ELEVATOR_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
-      ELEVATOR_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Inches.of(51).in(Units.Inches);
+      ELEVATOR_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Inches.of(66).in(Units.Inches);
       ELEVATOR_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
       ELEVATOR_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Inches.of(0)
           .in(Units.Inches);
@@ -232,13 +235,14 @@ public static class constElevator {
       ELEVATOR_CONFIG.Feedback.SensorToMechanismRatio = 0.4545;
       ELEVATOR_CONFIG.Slot0.kG = 0.3;
       ELEVATOR_CONFIG.Slot0.kS = 0.4;
-      ELEVATOR_CONFIG.Slot0.kP = 1;
+      // ELEVATOR_CONFIG.Slot0.kP = 1;
+      ELEVATOR_CONFIG.Slot0.kP = 0.3;
     }
 
-    public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(5);
-    public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(10);
-    public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(30);
-    public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(50);
+    public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(8.039062);
+    public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(16.946289);
+    public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(33.742188);
+    public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(58.888916);
     public static final Distance ALGAE_PREP_NET = Units.Inches.of(50);
     public static final Distance ALGAE_PREP_PROCESSOR_HEIGHT = Units.Inches.of(1);
     public static final Distance ALGAE_L3_CLEANING = Units.Inches.of(35);

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

@@ -35,13 +35,13 @@ public class RobotContainer {
   private final Drivetrain subDrivetrain = new Drivetrain();
 
   private final Hopper subHopper = new Hopper();
-  private final IntakeCoralHopper com_IntakeCoralHopper = new IntakeCoralHopper(subHopper);
 
   private final AlgaeIntake subAlgaeIntake = new AlgaeIntake();
   private final CoralOuttake subCoralOuttake = new CoralOuttake();
   private final Climber subClimber = new Climber();
   private final Elevator subElevator = new Elevator();
 
+  private final IntakeCoralHopper com_IntakeCoralHopper = new IntakeCoralHopper(subHopper, subCoralOuttake);
   private final Climb comClimb = new Climb(subClimber);
   private final PlaceCoral comPlaceCoral = new PlaceCoral(subCoralOuttake);
   private final PrepProcessor comPrepProcessor = new PrepProcessor(subElevator);
@@ -75,7 +75,7 @@ public RobotContainer() {
     NamedCommands.registerCommand("Prep Coral Station",
         Commands.runOnce(() -> subElevator.setPosition(Constants.constElevator.CORAL_L4_HEIGHT), subElevator));
 
-    NamedCommands.registerCommand("Get Coral Station Piece", new IntakeCoralHopper(subHopper));
+    NamedCommands.registerCommand("Get Coral Station Piece", new IntakeCoralHopper(subHopper, subCoralOuttake));
   }
 
   private void configureDriverBindings(SN_XboxController controller) {
@@ -85,6 +85,7 @@ private void configureDriverBindings(SN_XboxController controller) {
   }
 
   private void configureOperatorBindings(SN_XboxController controller) {
+    controller.btn_Start.onTrue(Commands.runOnce(() -> subElevator.resetSensorPosition(0)).ignoringDisable(true));
     controller.btn_Back.whileTrue(com_IntakeCoralHopper);
     // LT: Eat Algae
     controller.btn_LeftTrigger
@@ -103,9 +104,8 @@ private void configureOperatorBindings(SN_XboxController controller) {
     controller.btn_LeftBumper
         .whileTrue(comClimb);
 
-    // btn_East: Prep Net
     controller.btn_East
-        .onTrue(comPrepNet);
+        .onTrue(Commands.runOnce(() -> subElevator.setNeutral(), subElevator));
     // btn_South: Prep Processor
     controller.btn_South
         .onTrue(comPrepProcessor);
@@ -114,11 +114,10 @@ private void configureOperatorBindings(SN_XboxController controller) {
     controller.btn_West
         .whileTrue(comCleaningL3Reef);
 
-        // btn_North: Clean L2 Reef
+    // btn_North: Clean L2 Reef
     controller.btn_North
         .whileTrue(comCleaningL2Reef);
 
-
     // btn_A/B/Y/X: Set Elevator to Coral Levels
     controller.btn_A
         .onTrue(new PrepCoralLv(subElevator, Constants.constElevator.CORAL_L1_HEIGHT));

src/main/java/frc/robot/commands/states/IntakeCoralHopper.java

@@ -5,23 +5,27 @@
 package frc.robot.commands.states;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.CoralOuttake;
 import frc.robot.subsystems.Hopper;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class IntakeCoralHopper extends Command {
 
   Hopper subCoralIntake;
+  CoralOuttake subCoralOuttake;
 
   /** Creates a new IntakeCoralHopper. */
-  public IntakeCoralHopper(Hopper subHopper) {
+  public IntakeCoralHopper(Hopper subHopper, CoralOuttake subCoralOuttake) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.subCoralIntake = subHopper;
+    this.subCoralOuttake = subCoralOuttake;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-
+    subCoralOuttake.setCoralOuttake(Constants.constCoralOuttake.CORAL_OUTTAKE_SPEED);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -32,11 +36,12 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    subCoralOuttake.setCoralOuttake(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return true;
+    return subCoralOuttake.hasCoral();
   }
 }

src/main/java/frc/robot/commands/states/PrepCoralLv.java

@@ -20,6 +20,8 @@ public PrepCoralLv(Elevator subElevator, Distance height) {
     this.globalElevator = subElevator;
 
     this.globalDistance = height;
+
+    addRequirements(subElevator);
   }
 
   // Called when the command is initially scheduled.

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

@@ -7,7 +7,9 @@
 import com.ctre.phoenix6.hardware.CANrange;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.constCoralOuttake;
 import frc.robot.RobotMap.mapCoralOuttake;
@@ -22,6 +24,13 @@ public CoralOuttake() {
     outtakeMotor = new TalonFX(mapCoralOuttake.CORAL_OUTTAKE_MOTOR_CAN);
     outtakeMotor2 = new TalonFX(mapCoralOuttake.CORAL_OUTTAKE_MOTOR_CAN_2);
     coralSensor = new CANrange(mapCoralOuttake.CORAL_SENSOR_CAN);
+
+    configure();
+  }
+
+  public void configure() {
+    outtakeMotor.getConfigurator().apply(constCoralOuttake.CORAL_OUTTAKE_CONFIG);
+    outtakeMotor2.getConfigurator().apply(constCoralOuttake.CORAL_OUTTAKE_CONFIG);
   }
 
   public void setCoralOuttake(double speed) {
@@ -35,6 +44,8 @@ public boolean hasCoral() {
 
   @Override
   public void periodic() {
+    SmartDashboard.putNumber("CORAL SENSOR DISTANCE", coralSensor.getDistance().getValue().in(Units.Inches));
+    SmartDashboard.putBoolean("CORAL SENSOR HAS GP", hasCoral());
     // This method will be called once per scheduler run
   }
 }

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

@@ -5,6 +5,7 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
@@ -24,26 +25,29 @@ public class Elevator extends SubsystemBase {
   public Elevator() {
     leftMotorFollower = new TalonFX(mapElevator.LEFT_ELEVATOR_CAN);
     rightMotorLeader = new TalonFX(mapElevator.RIGHT_ELEVATOR_CAN);
-    
+
     configure();
   }
-  
-  public void configure(){
+
+  public void configure() {
     rightMotorLeader.getConfigurator().apply(constElevator.ELEVATOR_CONFIG);
     leftMotorFollower.getConfigurator().apply(constElevator.ELEVATOR_CONFIG);
   }
 
-
-  public Distance getElevatorPosition(){
+  public Distance getElevatorPosition() {
     return Units.Inches.of(rightMotorLeader.get());
   }
 
-
   public void setPosition(Distance height) {
     rightMotorLeader.setControl(new PositionVoltage(height.in(Units.Inches)));
     leftMotorFollower.setControl(new Follower(rightMotorLeader.getDeviceID(), true));
   }
 
+  public void setNeutral() {
+    rightMotorLeader.setControl(new NeutralOut());
+    leftMotorFollower.setControl(new NeutralOut());
+  }
+
   public void resetSensorPosition(double setpoint) {
     rightMotorLeader.setPosition(setpoint);
     leftMotorFollower.setPosition(setpoint);