diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed9c14d..543a420 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -192,10 +192,12 @@ public class RobotContainer { .withPosition(4, 1) .withWidget(BuiltInWidgets.kBooleanBox); + /* sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch) .withSize(1, 1) .withPosition(4, 0) .withWidget(BuiltInWidgets.kBooleanBox); + */ } public Command getAutonomousCommand() { @@ -211,8 +213,8 @@ public class RobotContainer { ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition ) - .andThen(manipulator.runUntilCollected(1, true)); - } + .andThen(manipulator.runUntilCollected(1)); + } /** * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected @@ -225,7 +227,7 @@ public class RobotContainer { l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition ) - .andThen(manipulator.runUntilCollected(1, false)); + .andThen(manipulator.runUntilCollected(1)); } /** diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index d2d5263..e401bd3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -17,7 +17,8 @@ public class ElevatorConstants { public static final int kTopLimitSwitchID = 0; public static final int kBottomLimitSwitchID = 0; - public static final double kEncoderConversionFactor = 0; + // 60/12 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position + public static final double kEncoderConversionFactor = 11/60 * Math.PI * 1.75 * 2; public static final int kMotorAmpsMax = 40; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index cc91d7b..f6dcd52 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -19,6 +19,8 @@ public class ManipulatorPivotConstants { public static final int kMotorAmpsMax = 40; + public static final double kPivotConversion = 12/60 * 20/60 * 28/12; + public static final double kPivotMaxVelocity = 0; public static final double kPositionalP = 0; @@ -81,6 +83,7 @@ public class ManipulatorPivotConstants { motorConfig .smartCurrentLimit(kMotorAmpsMax) .idleMode(kIdleMode); + motorConfig.encoder.positionConversionFactor(kPivotConversion); } diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index bff58d8..c4f6447 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -16,7 +16,7 @@ public class Manipulator extends SubsystemBase { private SparkMax manipulatorMotor; private DigitalInput coralBeamBreak; - private DigitalInput algaeBeamBreak; + // private DigitalInput algaeBeamBreak; public Manipulator() { manipulatorMotor = new SparkMax( @@ -31,7 +31,7 @@ public class Manipulator extends SubsystemBase { ); coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); - algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); + // algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); } /** @@ -42,10 +42,8 @@ public class Manipulator extends SubsystemBase { */ public Command defaultCommand() { return run(() -> { - manipulatorMotor.set( - algaeBeamBreak.get() ? 0.1 : 0 - ); - }); + runUntilCollected(0.1); + }); } /** @@ -70,19 +68,15 @@ public class Manipulator extends SubsystemBase { * @param coral Is the object a coral? (True = Coral, False = Algae) * @return Returns a command that sets the speed of the motor */ - public Command runUntilCollected(double speed, boolean coral) { + public Command runUntilCollected(double speed) { return run(() -> { manipulatorMotor.set( - coral ? speed : speed * -1 + speed * -1 ); - }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get()); + }).until(() -> coralBeamBreak.get()); } public boolean getCoralBeamBreak() { return coralBeamBreak.get(); } - - public boolean getAlgaePhotoSwitch() { - return algaeBeamBreak.get(); - } }