diff --git a/src/main/java/frc/robot/commands/ExtendClimberCommand.java b/src/main/java/frc/robot/commands/ExtendClimberCommand.java new file mode 100644 index 0000000..d77a562 --- /dev/null +++ b/src/main/java/frc/robot/commands/ExtendClimberCommand.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ClimberSubsystem; + + +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ClimberSubsystem; + +public class ExtendClimberCommand extends Command { + private final ClimberSubsystem climberSubsystem; + + private RelativeEncoder leftEncoder; + +// private RelativeEncoder rightEncoder; + + double leftEncoderPos; +// double rightEncoderPos; + + public ExtendClimberCommand(ClimberSubsystem climberSubsystem) { + this.climberSubsystem = climberSubsystem; + this.leftEncoder = climberSubsystem.getLeftEncoder(); +// this.rightEncoder = climberSubsystem.getRightEncoder(); + addRequirements(climberSubsystem); + } + + @Override + public void execute() { + + leftEncoderPos = leftEncoder.getPosition(); +// rightEncoderPos = rightEncoder.getPosition(); + + if (leftEncoderPos > 3.0) { + climberSubsystem.hold(); + } else { + climberSubsystem.extend(); + } + + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 55dc93b..8d64a95 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -3,12 +3,15 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IndexerSubsystem indexerSubsystem; + private LEDSubsystem ledSubsystem; + /** * Command to run the intake * @param intakeSubsystem The instance of {@link IntakeSubsystem} @@ -26,6 +29,7 @@ public void execute() { if (!indexerSubsystem.isCenter()) { intakeSubsystem.setSpeed(.9); indexerSubsystem.rotateAllWheelsPercent(.9); + ledSubsystem.setLED(-0.71); } else { intakeSubsystem.setSpeed(0); indexerSubsystem.rotateAllWheelsPercent(0); diff --git a/src/main/java/frc/robot/commands/RetractClimberCommand.java b/src/main/java/frc/robot/commands/RetractClimberCommand.java new file mode 100644 index 0000000..cf59e6e --- /dev/null +++ b/src/main/java/frc/robot/commands/RetractClimberCommand.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ClimberSubsystem; + +public class RetractClimberCommand extends Command { + private final ClimberSubsystem climberSubsystem; + + private final RelativeEncoder leftEncoder; + +// private RelativeEncoder rightEncoder; + + + public RetractClimberCommand(ClimberSubsystem climberSubsystem) { + this.climberSubsystem = climberSubsystem; + this.leftEncoder = climberSubsystem.getLeftEncoder(); +// this.rightEncoder = climberSubsystem.getRightEncoder(); + addRequirements(climberSubsystem); + } + + @Override + public void execute() { + double leftEncoderPos = leftEncoder.getPosition(); +// double rightEncoderPos = rightEncoder.getPosition(); + + climberSubsystem.retract(); + + if (leftEncoderPos < 1.0) { + climberSubsystem.hold(); + } else { + climberSubsystem.retract(); + } + + } +} diff --git a/src/main/java/frc/robot/commands/SetLEDCommand.java b/src/main/java/frc/robot/commands/SetLEDCommand.java index 2392832..cd8e237 100644 --- a/src/main/java/frc/robot/commands/SetLEDCommand.java +++ b/src/main/java/frc/robot/commands/SetLEDCommand.java @@ -18,12 +18,39 @@ public SetLEDCommand(LEDSubsystem subsystem, LEDConstants.Status status) { @Override public void execute() { switch(status) { - case TEST_1: - //Confetti - subsystem.setLED(-0.87); - case TEST_2: - //Sinelon, Forest Palette - subsystem.setLED(-0.71); +// case TEST_1: +// //Confetti +// subsystem.setLED(-0.87); +// case TEST_2: +// //Sinelon, Forest Palette +// subsystem.setLED(-0.71); + case IDLE: + //Solid, Gold + subsystem.setLED(0.69); + case INTAKINGINGING: + //Solid, Aqua + subsystem.setLED(0.81); + case MOVING: + //Solid, White + subsystem.setLED(0.93); + case DISABLED: + //Solid, Red + subsystem.setLED(0.61); + case SHOOTING: + //Solid, Lime + subsystem.setLED(0.73); + case VISION_MOVING: + //Solid, Blue + subsystem.setLED(0.87); + case SPINUP: + //Solid, Violet + subsystem.setLED(0.91); + case CLIMBER_EXTENDING: + //Shot, Blue + subsystem.setLED(-0.83); + case CLIMBER_RETRACTING: + //Shot, Red + subsystem.setLED(-0.85); } } diff --git a/src/main/java/frc/robot/commands/TranslateCommand.java b/src/main/java/frc/robot/commands/TranslateCommand.java new file mode 100644 index 0000000..b956088 --- /dev/null +++ b/src/main/java/frc/robot/commands/TranslateCommand.java @@ -0,0 +1,26 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveSubsystem; + +public class TranslateCommand extends Command { + + private final SwerveSubsystem swerveSubsystem; + private final double metersPerSecond; + + public TranslateCommand(SwerveSubsystem swerveSubsystem, double metersPerSecond) { + this.swerveSubsystem = swerveSubsystem; + this.metersPerSecond = metersPerSecond; + } + + + @Override + + public void execute() { + swerveSubsystem.drive(metersPerSecond, 0.0, 0.0, false, true); + } + + public double getMetersPerSecond() { + return metersPerSecond; + } +} diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java new file mode 100644 index 0000000..afc7218 --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class ClimberConstants { + public static final int leftSideID = 13; + public static final int rightSideID = 16; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index ecb4aae..a46be0f 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -27,7 +27,7 @@ public class DrivetrainConstants { new Translation2d(-wheelBase / 2, -trackWidth / 2) ); - public static final double frontLeftChassisAngularOffset = 5.516 + Math.PI/2 + Math.PI; + public static final double frontLeftChassisAngularOffset = 5.516 - Math.PI/2; public static final double frontRightChassisAngularOffset = 6.107; public static final double rearLeftChassisAngularOffset = 0.876 + Math.PI; public static final double rearRightChassisAngularOffset = 1.036 + Math.PI/2; @@ -45,13 +45,17 @@ public class DrivetrainConstants { public static final boolean gyroReversed = false; public static final boolean turningEncoderReversed = true; - public static final int drivingMotorPinionTeeth = 13; + public static final int drivingMotorPinionTeeth = 16; - public static final double freeSpeedRpm = 5676.0; + public static final int spurGearTeeth = 20; + + + //Free Speed RPM: 6784 + public static final double freeSpeedRpm = 6784.0; public static final double drivingMotorFreeSpeedRps = freeSpeedRpm / 60.0; public static final double wheelDiameterMeters = 0.0762; public static final double wheelCircumferenceMeters = wheelDiameterMeters * Math.PI; - public static final double drivingMotorReduction = (45.0 * 22) / (drivingMotorPinionTeeth * 15); + public static final double drivingMotorReduction = (45.0 * spurGearTeeth) / (drivingMotorPinionTeeth * 15); public static final double driveWheelFreeSpeedRps = (drivingMotorFreeSpeedRps * wheelCircumferenceMeters) / drivingMotorReduction; public static final double drivingEncoderPositionFactor = (wheelDiameterMeters * Math.PI) / drivingMotorReduction; diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 5e2c77f..09a463b 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -7,7 +7,13 @@ public static enum Status { MOVING, VISION_MOVING, TEST_1, - TEST_2 + TEST_2, + SHOOTING, + INTAKINGINGING, + SPINUP, + CLIMBER_EXTENDING, + CLIMBER_RETRACTING } -} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..7508af0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ClimberConstants; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.constants.DrivetrainConstants; +import frc.robot.commands.DriveCommands; + + +public class ClimberSubsystem extends SubsystemBase { + private final CANSparkMax leftSide = new CANSparkMax(ClimberConstants.leftSideID, CANSparkLowLevel.MotorType.kBrushless); + // private CANSparkMax rightSide = new CANSparkMax(ClimberConstants.leftSideID, CANSparkLowLevel.MotorType.kBrushless); + private final RelativeEncoder leftSideEncoder = leftSide.getEncoder(); +// private final RelativeEncoder rightSideEncoder = rightSide.getEncoder(); + + public RelativeEncoder getLeftEncoder() { + return leftSideEncoder; + } + +// public RelativeEncoder getRightEncoder() { +// return rightSideEncoder; +// } + + public void extend() { + //rightSide.set(0.1); + leftSide.set(0.1); + } + + public void retract() { + //rightSide.set(-0.1); + leftSide.set(-0.1); + } + + + /** + * Put the brakes on the climber motors. + */ + public void hold() { + leftSide.set(0.0); + //rightSide.set(0.0); + leftSide.setIdleMode(CANSparkBase.IdleMode.kBrake); + //rightSide.setIdleMode(CANSparkBase.IdleMode.kBrake); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 0633e65..2ccd57a 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -16,4 +16,4 @@ public void periodic() { public void setLED(double value) { BlinkinDriver.set(value); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 961009b..7caf4f2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -321,6 +321,21 @@ private void resetOdometry(Pose2d pose) { public void drive(double forwardMetersPerSecond, double sidewaysMetersPerSecond, double radiansPerSecond, boolean fieldRelative, boolean rateLimit) { // forward is xspeed, sideways is yspeed +// double xSpeedCommanded = 0.0; +// double ySpeedCommanded = 0.0; +// +// double[] areaPoints = {0.0, 0.0, 2.0, 2.0}; +// double poseX = getPose().getX(); +// double poseY = getPose().getY(); +// +// boolean aboveMinPoints = poseX >= areaPoints[0] && poseY >= areaPoints[1]; +// boolean belowMaxPoints = poseX <= areaPoints[2] && poseY <= areaPoints[3]; +// +// if (aboveMinPoints && belowMaxPoints) { +// xSpeedCommanded /= 2; +// ySpeedCommanded /= 2; +// } + double xSpeedCommanded; double ySpeedCommanded;