diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b4b534..5542c28 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,14 +5,14 @@ package frc.robot; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.commands.ElevatorCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.ElevatorToPosition; import frc.robot.commands.TankDriveCommand; import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorConstants; public class RobotContainer { /** @@ -36,28 +36,24 @@ public class RobotContainer { * Up[-1,1]Down */ private CommandJoystick driverController; - private CommandJoystick opController; + private CommandXboxController opController; private final Drivetrain drive = new Drivetrain(); - private final Elevator elevate = new Elevator(); + private final Elevator elevator = new Elevator(); public RobotContainer() { driverController = new CommandJoystick(0); - opController = new CommandJoystick(1); + opController = new CommandXboxController(1); drive.setDefaultCommand(new TankDriveCommand(drive, ()->driverController.getRawAxis(1), ()->driverController.getRawAxis(5))); - new JoystickButton(opController.getHID(), 6).onTrue((new ElevatorCommand(elevate, true).withInterruptBehavior(InterruptionBehavior.kCancelSelf))); - new JoystickButton(opController.getHID(), 5).onTrue((new ElevatorCommand(elevate, false).withInterruptBehavior(InterruptionBehavior.kCancelSelf))); - configureBindings(); - } - - private void configureBindings() { - + opController.povUp().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.topHeight)); + opController.povLeft().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.middleHeight)); + opController.povDown().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.bottomHeight)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java deleted file mode 100644 index 38cc129..0000000 --- a/src/main/java/frc/robot/commands/ElevatorCommand.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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 edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; - -public class ElevatorCommand extends Command { - - private Elevator elevate; - private boolean goingUp; - private final ArmPose[] poses = {new ArmPose(0, 0), new ArmPose(.25, 0), new ArmPose(.5, -.25)}; - private int currentPose; - - /** Creates a new TankDrive. */ - public ElevatorCommand(Elevator elevate, boolean goingUp) { - addRequirements(elevate); - this.elevate = elevate; - this.goingUp = goingUp; - currentPose = 0; - } - - // 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 (goingUp){ - if (currentPose != poses.length - 1){ - currentPose++; - } - } - else{ - if (currentPose != 0){ - currentPose--; - } - } - elevate.setPose(poses[currentPose].height, poses[currentPose].wristAngle); - } - - // 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; - } - /** - * @param height - * @param wristAngle - */ - record ArmPose(double height, double wristAngle){} -} diff --git a/src/main/java/frc/robot/commands/ElevatorToPosition.java b/src/main/java/frc/robot/commands/ElevatorToPosition.java new file mode 100644 index 0000000..fcbbcb9 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorToPosition.java @@ -0,0 +1,28 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +public class ElevatorToPosition extends Command { + + private final Elevator elevator; + private final double height; + + public ElevatorToPosition(Elevator elevator, double height) { + this.elevator = elevator; + this.height = height; + addRequirements(elevator); + } + + @Override + public void execute() { + elevator.setPosition(height); + } + + @Override + public boolean isFinished() { + //if we hit a hard stop, cancel the command + return elevator.hasHitHardStop(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 3e4b1e3..21ae18c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,81 +1,59 @@ package frc.robot.subsystems.elevator; -import com.revrobotics.spark.ClosedLoopSlot; +import static edu.wpi.first.units.Units.MetersPerSecond; + import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import com.revrobotics.spark.SparkMax; - import frc.robot.util.SparkUtil; public class Elevator extends SubsystemBase{ private final SparkMax heightMotor; - private final SparkMax armMotor; - private final SparkClosedLoopController closedLoopController; - private final SparkClosedLoopController armLoopController; + + private final SparkClosedLoopController heightPID; + + public Elevator(){ heightMotor = new SparkMax(ElevatorConstants.heightMotorCANID, MotorType.kBrushless); - armMotor = new SparkMax(ElevatorConstants.armMotorCANID, MotorType.kBrushless); - - closedLoopController = heightMotor.getClosedLoopController(); - armLoopController = armMotor.getClosedLoopController(); + heightPID = heightMotor.getClosedLoopController(); SparkMaxConfig motorConfig = new SparkMaxConfig(); motorConfig.smartCurrentLimit(50).idleMode(IdleMode.kBrake).inverted(false); motorConfig.encoder.positionConversionFactor(ElevatorConstants.positionConversionFactor); - /* - * Configure the closed loop controller. We want to make sure we set the - * feedback sensor as the primary encoder. - */ motorConfig.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - // Set PID values for height control. We don't need to pass a closed loop - // slot, as it will default to slot 0. .p(0.1) .i(0) .d(0) .outputRange(-1, 1); - SparkUtil.tryUntilOk(5, - () -> heightMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - motorConfig.encoder.positionConversionFactor(ElevatorConstants.armPositionConversionFactor); - motorConfig.closedLoop - // Set PID values for arm control in slot 1 - .p(0.1, ClosedLoopSlot.kSlot0) - .i(0, ClosedLoopSlot.kSlot0) - .d(0, ClosedLoopSlot.kSlot0) - .outputRange(-1, 1, ClosedLoopSlot.kSlot0); - SparkUtil.tryUntilOk(5, - () -> armMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - + SparkUtil.tryUntilOk( + 5, + () -> heightMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); } - /** - * Set the motion of the elevator. Positive is up. - * @param speed - */ - public void run(double speed){ - heightMotor.set(speed); + public boolean hasHitHardStop() { + return + heightMotor.getOutputCurrent() > ElevatorConstants.autoStopCurrentThreshold + && + heightMotor.getEncoder().getVelocity() < ElevatorConstants.autoStopVelocityThreshold.in(MetersPerSecond); } /** - * Sets the elevator and accompanying arm's setpoints. I have never used Rev's built-in PID, so I can only hope this works. - * @param heightPosition Elevator's setpoint - * @param armAngle Arm's setpoint + * Sets the elevators setpoint. + * @param height Position to move towards */ - public void setPose(double heightPosition, double armAngle){ - closedLoopController.setReference(heightPosition, ControlType.kPosition, ClosedLoopSlot.kSlot0); - armLoopController.setReference(armAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); + public void setPosition(double height){ + heightPID.setReference(height, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index a110c34..b5de72d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,9 +1,17 @@ package frc.robot.subsystems.elevator; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.measure.LinearVelocity; + public class ElevatorConstants { public static final int heightMotorCANID = 6; - public static final int armMotorCANID = 7; public static final double positionConversionFactor = 1; //TODO: measure this - public static final double armPositionConversionFactor = 1; //TODO: measure this + public static final double autoStopCurrentThreshold = 15; + public static final LinearVelocity autoStopVelocityThreshold = MetersPerSecond.of(0.1); //TODO measure + + public static final double topHeight = 0; + public static final double middleHeight = 0; + public static final double bottomHeight = 0; } diff --git a/src/main/java/frc/robot/subsystems/wrist/MoveWrist.java b/src/main/java/frc/robot/subsystems/wrist/MoveWrist.java new file mode 100644 index 0000000..9bd4206 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/MoveWrist.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.wrist; + +import edu.wpi.first.wpilibj2.command.Command; + +public class MoveWrist extends Command { + + private final Wrist wrist; + private final boolean direction; + + public MoveWrist(Wrist wrist, boolean moveOut) { + this.wrist = wrist; + direction = moveOut; + } + + @Override + public void execute() { + wrist.rotate(WristConstants.wristSpeed * (direction ? 1 : -1)); + } + + @Override + public boolean isFinished() { + return wrist.hasHitHardStop(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java new file mode 100644 index 0000000..64a91cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/Wrist.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.wrist; + +import static edu.wpi.first.units.Units.RPM; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.SparkUtil; + +public class Wrist extends SubsystemBase { + + private final SparkMax motor; + + public Wrist() { + motor = new SparkMax(WristConstants.motorCANId, MotorType.kBrushless); + + SparkMaxConfig config = new SparkMaxConfig(); + config.smartCurrentLimit(0).idleMode(IdleMode.kBrake); + config.encoder.velocityConversionFactor(0); + + SparkUtil.tryUntilOk( + 5, + () -> motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); + } + + public boolean hasHitHardStop() { + return + motor.getOutputCurrent() > WristConstants.currentAutoStopThreshold + && + motor.getEncoder().getVelocity() < WristConstants.autoStopVelocityThreshold.in(RPM); + } + + /** + * Rotate the wrist. Positive is away from the robot. + * @param speed + */ + public void rotate(double speed) { + motor.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristConstants.java b/src/main/java/frc/robot/subsystems/wrist/WristConstants.java new file mode 100644 index 0000000..98c83f3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/WristConstants.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.wrist; + +import static edu.wpi.first.units.Units.RPM; + +import edu.wpi.first.units.measure.AngularVelocity; + +public class WristConstants { + public static final int motorCANId = 7; + public static final double currentAutoStopThreshold = 15; + public static final AngularVelocity autoStopVelocityThreshold = RPM.of(10); //TODO measure + + public static final double wristSpeed = 0.75; +}