diff --git a/src/main/java/frc/team4909/robot/Robot.java b/src/main/java/frc/team4909/robot/Robot.java index 078efdb..c68cb97 100644 --- a/src/main/java/frc/team4909/robot/Robot.java +++ b/src/main/java/frc/team4909/robot/Robot.java @@ -3,24 +3,23 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DriverStation; import frc.team4909.robot.operator.controllers.BionicF310; import frc.team4909.robot.sensors.LidarLitePWM; import frc.team4909.robot.sensors.Stream; -import frc.team4909.robot.subsystems.StiltWheel.StiltWheelSubsystem; -import frc.team4909.robot.subsystems.StiltWheel.commands.MoveStiltWheels; -import frc.team4909.robot.subsystems.climber.ClimberSubsystem; -import frc.team4909.robot.subsystems.climber.commands.*; +import frc.team4909.robot.subsystems.stiltwheel.StiltWheelSubsystem; +import frc.team4909.robot.subsystems.stiltwheel.commands.MoveStiltWheels; +import frc.team4909.robot.subsystems.stilts.StiltSubsystem; +import frc.team4909.robot.subsystems.stilts.commands.*; import frc.team4909.robot.subsystems.drivetrain.DriveTrainSubsystem; import frc.team4909.robot.subsystems.drivetrain.commands.InvertDriveDirection; import frc.team4909.robot.subsystems.drivetrain.commands.TogglePreciseMode; import frc.team4909.robot.subsystems.elevator.ElevatorSubsystem; import frc.team4909.robot.subsystems.elevator.commands.*; -import frc.team4909.robot.subsystems.elevatorarm.ElevatorArmSubsystem; -import frc.team4909.robot.subsystems.elevatorarm.commands.*; +import frc.team4909.robot.subsystems.wrist.WristSubsystem; +import frc.team4909.robot.subsystems.wrist.commands.*; import frc.team4909.robot.subsystems.intake.IntakeSubsystem; import frc.team4909.robot.subsystems.intake.commands.*; @@ -68,28 +67,14 @@ public class Robot extends TimedRobot { public static DriveTrainSubsystem drivetrainSubsystem; public static IntakeSubsystem intakeSubsystem; public static ElevatorSubsystem elevatorSubsystem; - public static ElevatorArmSubsystem elevatorArmSubsystem; - public static ClimberSubsystem climberSubsystem; + public static WristSubsystem wristSubsystem; + public static StiltSubsystem stiltSubsystem; public static StiltWheelSubsystem stiltWheelSubsystem; public static Compressor c; // Sensors public static LidarLitePWM lidar; - /** - * map a number from one range to another - * - * @param {num} value the value to be mapped - * @param {num} old_min the minimum of value - * @param {num} old_max the maximum of value - * @param {num} new_min the new minimum value - * @param {num} new_max the new maximum value - * @return {num} the value remaped on the range [new_min new_max] - */ - public static double map(double value, double old_min, double old_max, double new_min, double new_max) { - return (value - old_min) / (old_max - old_min) * (new_max - new_min) + new_min; - } - /** * This function is run when the robot is first started up and should be used * for any initialization code. @@ -113,8 +98,8 @@ public void robotInit() { drivetrainSubsystem = new DriveTrainSubsystem(); intakeSubsystem = new IntakeSubsystem(); elevatorSubsystem = new ElevatorSubsystem(); - elevatorArmSubsystem = new ElevatorArmSubsystem(); - climberSubsystem = new ClimberSubsystem(); + wristSubsystem = new WristSubsystem(); + stiltSubsystem = new StiltSubsystem(); stiltWheelSubsystem = new StiltWheelSubsystem(); // Sensors @@ -216,6 +201,7 @@ public void robotPeriodic() { } public void autonomousInit() { + new MoveUpToLimit().start(); // elevatorSubsystem.setSensorZero(); // elevatorArmSubsystem.setSensorZero(); // climberSubsystem.setSensorZero(); @@ -240,8 +226,8 @@ public void disabledPeriodic() { Scheduler.getInstance().run(); Robot.elevatorSubsystem.updateHoldingPos(); - Robot.elevatorArmSubsystem.holdingPosition = Robot.elevatorArmSubsystem.getPosition(); - Robot.climberSubsystem.updateHoldingPos(); + Robot.wristSubsystem.updateHoldingPos(); + Robot.stiltSubsystem.updateHoldingPos(); } public void teleopPeriodic() { diff --git a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/StiltWheelSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/StiltWheelSubsystem.java index ab5584b..29513d1 100644 --- a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/StiltWheelSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/StiltWheelSubsystem.java @@ -1,12 +1,9 @@ -package frc.team4909.robot.subsystems.StiltWheel; +package frc.team4909.robot.subsystems.stiltwheel; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; - -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; -import frc.team4909.robot.Robot; import frc.team4909.robot.RobotMap; -import frc.team4909.robot.subsystems.StiltWheel.commands.Default_StiltWheelStop; +import frc.team4909.robot.subsystems.stiltwheel.commands.Default_StiltWheelStop; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/Default_StiltWheelStop.java b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/Default_StiltWheelStop.java index e273ca9..667ce76 100644 --- a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/Default_StiltWheelStop.java +++ b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/Default_StiltWheelStop.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.StiltWheel.commands; +package frc.team4909.robot.subsystems.stiltwheel.commands; import edu.wpi.first.wpilibj.command.Command; import frc.team4909.robot.Robot; diff --git a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/MoveStiltWheels.java b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/MoveStiltWheels.java index de496b8..c78675a 100644 --- a/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/MoveStiltWheels.java +++ b/src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/MoveStiltWheels.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.StiltWheel.commands; +package frc.team4909.robot.subsystems.stiltwheel.commands; import edu.wpi.first.wpilibj.command.Command; import frc.team4909.robot.Robot; diff --git a/src/main/java/frc/team4909/robot/subsystems/drivetrain/DriveTrainSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/drivetrain/DriveTrainSubsystem.java index 4a9aa9a..4ca9047 100644 --- a/src/main/java/frc/team4909/robot/subsystems/drivetrain/DriveTrainSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/drivetrain/DriveTrainSubsystem.java @@ -9,10 +9,9 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team4909.robot.RobotMap; -import frc.team4909.robot.operator.controllers.BionicF310; import frc.team4909.robot.Robot; import frc.team4909.robot.RobotConstants; -import frc.team4909.robot.subsystems.drivetrain.commands.Drive; +import frc.team4909.robot.subsystems.drivetrain.commands.Default_Drive; public class DriveTrainSubsystem extends Subsystem { CANSparkMax frontLeftSparkMax, rearLeftSparkMax, frontRightSparkMax, rearRightSparkMax; @@ -88,7 +87,7 @@ public void togglePreciseMode() { } protected void initDefaultCommand() { - setDefaultCommand(new Drive()); + setDefaultCommand(new Default_Drive()); } @Override diff --git a/src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Drive.java b/src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Default_Drive.java similarity index 90% rename from src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Drive.java rename to src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Default_Drive.java index 0e5d37b..66e423b 100644 --- a/src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Drive.java +++ b/src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Default_Drive.java @@ -4,8 +4,8 @@ import frc.team4909.robot.Robot; import frc.team4909.robot.operator.controllers.BionicF310; -public class Drive extends Command { - public Drive() { +public class Default_Drive extends Command { + public Default_Drive() { requires(Robot.drivetrainSubsystem); } diff --git a/src/main/java/frc/team4909/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/elevator/ElevatorSubsystem.java index ca27134..dacf6f3 100644 --- a/src/main/java/frc/team4909/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/elevator/ElevatorSubsystem.java @@ -6,13 +6,9 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; - -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.ControlMode; - -import frc.team4909.robot.Robot; import frc.team4909.robot.RobotConstants; import frc.team4909.robot.RobotMap; import frc.team4909.robot.subsystems.elevator.commands.Default_ElevHoldPos; diff --git a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/ElevatorArmOperatorControl.java b/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/ElevatorArmOperatorControl.java deleted file mode 100644 index 04210e1..0000000 --- a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/ElevatorArmOperatorControl.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.team4909.robot.subsystems.elevatorarm.commands; - -import frc.team4909.robot.Robot; -import frc.team4909.robot.RobotConstants; -import frc.team4909.robot.operator.controllers.BionicF310; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class ElevatorArmOperatorControl extends Command { - - // private int holdingPosition = Robot.elevatorArmSubsystem.getPosition(); - - public ElevatorArmOperatorControl() { - requires(Robot.elevatorArmSubsystem); - SmartDashboard.putString("Wrist - Status", "Constructor"); - } - - @Override - public void execute() { - // Sets speed to manipulator gamepad right Y stick value - double moveSpeed = -Robot.manipulatorGamepad.getThresholdAxis(BionicF310.RY) - * RobotConstants.elevatorArmSpeedMultiplier; - - if (moveSpeed == 0) { // If Y-stick value is not moving, HOLD position - SmartDashboard.putString("Wrist - Status", "Hold"); - Robot.elevatorArmSubsystem.setPosition(Robot.elevatorArmSubsystem.holdingPosition); - } else { // Set speed to Y-stick value and HOLD position - SmartDashboard.putString("Wrist - Status", "Move"); - Robot.elevatorArmSubsystem.elevatorArmSetSpeed(moveSpeed); - Robot.elevatorArmSubsystem.holdingPosition = Robot.elevatorArmSubsystem.getPosition(); - } - - } - - @Override - protected boolean isFinished() { - return false; - } - - protected void interrupted() { - SmartDashboard.putString("Wrist - Status", "Interrupted"); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetAngle.java b/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetAngle.java deleted file mode 100644 index e3bcd20..0000000 --- a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetAngle.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.team4909.robot.subsystems.elevatorarm.commands; - -import edu.wpi.first.wpilibj.command.InstantCommand; -import frc.team4909.robot.subsystems.elevatorarm.ElevatorArmSubsystem; - -public class SetAngle extends InstantCommand { - private final int setpoint; - private final ElevatorArmSubsystem elevatorArm; - - public SetAngle(int setpoint, ElevatorArmSubsystem elevatorArm) { - this.setpoint = setpoint; - this.elevatorArm = elevatorArm; - } - - @Override - public void initialize() { - elevatorArm.holdingPosition = setpoint; - } - -} \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristSpeed.java b/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristSpeed.java deleted file mode 100644 index 5bd0d33..0000000 --- a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristSpeed.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.team4909.robot.subsystems.elevatorarm.commands; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.InstantCommand; -import frc.team4909.robot.Robot; -import frc.team4909.robot.subsystems.elevatorarm.ElevatorArmSubsystem; - -public class SetWristSpeed extends Command { - - private double speed = 0; - - public SetWristSpeed(double speed1) { - requires(Robot.elevatorArmSubsystem); - speed = speed1; - } - - @Override - public void initialize() { - Robot.elevatorArmSubsystem.elevatorArmSetSpeed(speed); - } - @Override - protected void end() { - Robot.elevatorArmSubsystem.elevatorArmSetSpeed(0); - Robot.elevatorArmSubsystem.holdingPosition = Robot.elevatorArmSubsystem.getPosition(); - } - - @Override - protected boolean isFinished() { - return false; - } - -} \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java index e0b3835..3c99282 100644 --- a/src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java @@ -10,8 +10,8 @@ import frc.team4909.robot.Robot; import frc.team4909.robot.RobotConstants; import frc.team4909.robot.RobotMap; -import frc.team4909.robot.subsystems.intake.commands.CargoIntakeHold; -import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose; +import frc.team4909.robot.subsystems.intake.commands.Default_CargoIntakeHold; +import frc.team4909.robot.subsystems.intake.commands.Default_HatchPanelIntakeClose; public class IntakeSubsystem extends Subsystem { DoubleSolenoid hatchPanelSolenoid; @@ -37,13 +37,7 @@ public void hatchPanelIntakeClose() { } public void holdCargoIntake(){ - // if(hasCargo()){ - // setCargoIntakeSpeed(RobotConstants.cargoIntakeHoldSpeed); - // } else { - // setCargoIntakeSpeed(0.0625); - setCargoIntakeSpeed(RobotConstants.cargoIntakeHoldSpeed); - // } } public void setCargoIntakeSpeed(double speed) { @@ -86,8 +80,8 @@ protected void initDefaultCommand() { // Revert to Closed by Default, Will Simplify While // Held/Toggle Open Commands in Future - addParallel(new HatchPanelIntakeClose()); - addParallel(new CargoIntakeHold()); + addParallel(new Default_HatchPanelIntakeClose()); + addParallel(new Default_CargoIntakeHold()); } }); } diff --git a/src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeHold.java b/src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_CargoIntakeHold.java similarity index 81% rename from src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeHold.java rename to src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_CargoIntakeHold.java index 6c4ff67..e350939 100644 --- a/src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeHold.java +++ b/src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_CargoIntakeHold.java @@ -4,8 +4,8 @@ import frc.team4909.robot.Robot; import frc.team4909.robot.RobotConstants; -public class CargoIntakeHold extends Command{ - public CargoIntakeHold(){ +public class Default_CargoIntakeHold extends Command{ + public Default_CargoIntakeHold(){ requires(Robot.intakeSubsystem); } diff --git a/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeClose.java b/src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_HatchPanelIntakeClose.java similarity index 71% rename from src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeClose.java rename to src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_HatchPanelIntakeClose.java index 484eadf..0eb05d6 100644 --- a/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeClose.java +++ b/src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_HatchPanelIntakeClose.java @@ -3,9 +3,9 @@ import edu.wpi.first.wpilibj.command.InstantCommand; import frc.team4909.robot.Robot; -public class HatchPanelIntakeClose extends InstantCommand { +public class Default_HatchPanelIntakeClose extends InstantCommand { - public HatchPanelIntakeClose() { + public Default_HatchPanelIntakeClose() { requires(Robot.intakeSubsystem); } diff --git a/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java b/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java index df37a57..357d3f6 100644 --- a/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java +++ b/src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java @@ -1,7 +1,6 @@ package frc.team4909.robot.subsystems.intake.commands; import edu.wpi.first.wpilibj.command.Command; -import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose; import frc.team4909.robot.Robot; public class HatchPanelIntakeOpen extends Command { diff --git a/src/main/java/frc/team4909/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/stilts/StiltSubsystem.java similarity index 85% rename from src/main/java/frc/team4909/robot/subsystems/climber/ClimberSubsystem.java rename to src/main/java/frc/team4909/robot/subsystems/stilts/StiltSubsystem.java index 27d3682..06021ea 100644 --- a/src/main/java/frc/team4909/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/stilts/StiltSubsystem.java @@ -1,20 +1,17 @@ -package frc.team4909.robot.subsystems.climber; +package frc.team4909.robot.subsystems.stilts; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; - -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team4909.robot.RobotMap; -import frc.team4909.robot.subsystems.climber.commands.Default_StiltsHoldPos; -import frc.team4909.robot.Robot; +import frc.team4909.robot.subsystems.stilts.commands.Default_StiltsHoldPos; import frc.team4909.robot.RobotConstants; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; -public class ClimberSubsystem extends Subsystem { +public class StiltSubsystem extends Subsystem { // All motor controllers should be private. // Methods that allow safe motion should be provided by the subsystem @@ -22,7 +19,7 @@ public class ClimberSubsystem extends Subsystem { private WPI_VictorSPX climberLiftSlave; private int holdingStiltsPosition = 0; - public ClimberSubsystem() { + public StiltSubsystem() { //super should always be called to ensure proper subystem initialization super(); climberLiftMaster = new WPI_TalonSRX(RobotMap.climberMasterSRXID); @@ -128,6 +125,21 @@ public void holdPosition() { setPosition(holdingStiltsPosition); } + public boolean isAtTop() { + //@todo not sure if fwd or reverse + return climberLiftMaster.getSensorCollection().isFwdLimitSwitchClosed(); + } + + public void setCurrentLimit(int maxAmps) { + if (maxAmps == 0) { + climberLiftMaster.enableCurrentLimit(false); + } else { + climberLiftMaster.enableCurrentLimit(true); + climberLiftMaster.configPeakCurrentLimit(0, RobotConstants.timeoutMs); + climberLiftMaster.configContinuousCurrentLimit(maxAmps, RobotConstants.timeoutMs); + } + } + diff --git a/src/main/java/frc/team4909/robot/subsystems/climber/commands/Default_StiltsHoldPos.java b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/Default_StiltsHoldPos.java similarity index 68% rename from src/main/java/frc/team4909/robot/subsystems/climber/commands/Default_StiltsHoldPos.java rename to src/main/java/frc/team4909/robot/subsystems/stilts/commands/Default_StiltsHoldPos.java index 236f326..33e2043 100644 --- a/src/main/java/frc/team4909/robot/subsystems/climber/commands/Default_StiltsHoldPos.java +++ b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/Default_StiltsHoldPos.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.climber.commands; +package frc.team4909.robot.subsystems.stilts.commands; import edu.wpi.first.wpilibj.command.Command; import frc.team4909.robot.Robot; @@ -7,11 +7,11 @@ public class Default_StiltsHoldPos extends Command { public Default_StiltsHoldPos() { - requires(Robot.climberSubsystem); + requires(Robot.stiltSubsystem); } @Override protected void initialize() { - Robot.climberSubsystem.holdPosition(); + Robot.stiltSubsystem.holdPosition(); } @Override diff --git a/src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveElevAndStitls.java b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveElevAndStitls.java similarity index 79% rename from src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveElevAndStitls.java rename to src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveElevAndStitls.java index 0d22cff..8931020 100644 --- a/src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveElevAndStitls.java +++ b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveElevAndStitls.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.climber.commands; +package frc.team4909.robot.subsystems.stilts.commands; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -14,7 +14,7 @@ public class MoveElevAndStitls extends Command { private BionicAxis axis; public MoveElevAndStitls(boolean isGoingUp) { - requires(Robot.climberSubsystem); + requires(Robot.stiltSubsystem); requires(Robot.elevatorSubsystem); this.isGoingUp = isGoingUp; if (isGoingUp) { @@ -24,7 +24,7 @@ public MoveElevAndStitls(boolean isGoingUp) { } } protected void initialize() { - startStiltPos = Robot.climberSubsystem.getPosition(); + startStiltPos = Robot.stiltSubsystem.getPosition(); startElevatorPos = Robot.elevatorSubsystem.getPosition(); } @@ -36,9 +36,9 @@ protected void execute() { double speed = Robot.climberGamepad.getThresholdAxis(axis); speed *= RobotConstants.climbBothSpeedMultiplier; - Robot.climberSubsystem.setStiltsClimbSpeed((isGoingUp ? 1 : -1) * speed * 2); // *2 because trigger only goes to .3 + Robot.stiltSubsystem.setStiltsClimbSpeed((isGoingUp ? 1 : -1) * speed * 2); // *2 because trigger only goes to .3 - int stiltDelta = Math.abs(startStiltPos - Robot.climberSubsystem.getPosition()); + int stiltDelta = Math.abs(startStiltPos - Robot.stiltSubsystem.getPosition()); // Elevator Drum is 1.3" Diameter, C = PI * D = Math.PI * 1.3 // Stilts pinion gear Pitch Diameter is 1.1" which is the circumference @@ -60,7 +60,7 @@ protected boolean isFinished() { protected void end() { Robot.elevatorSubsystem.setSpeed(0); Robot.elevatorSubsystem.updateHoldingPos(); - Robot.climberSubsystem.setSpeed(0); - Robot.climberSubsystem.updateHoldingPos(); + Robot.stiltSubsystem.setSpeed(0); + Robot.stiltSubsystem.updateHoldingPos(); } } \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveStiltsOnly.java b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveStiltsOnly.java similarity index 65% rename from src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveStiltsOnly.java rename to src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveStiltsOnly.java index 235d393..3f3bddc 100644 --- a/src/main/java/frc/team4909/robot/subsystems/climber/commands/MoveStiltsOnly.java +++ b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveStiltsOnly.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.climber.commands; +package frc.team4909.robot.subsystems.stilts.commands; import edu.wpi.first.wpilibj.command.Command; import frc.team4909.robot.Robot; @@ -7,20 +7,20 @@ public class MoveStiltsOnly extends Command { public MoveStiltsOnly() { - requires(Robot.climberSubsystem); + requires(Robot.stiltSubsystem); } protected void initialize() { } protected void execute() { double speed = Robot.climberGamepad.getThresholdAxis(BionicF310.RY); - Robot.climberSubsystem.setStiltsClimbSpeed(-1 * speed * RobotConstants.climbSpeedMultiplier); + Robot.stiltSubsystem.setStiltsClimbSpeed(-1 * speed * RobotConstants.climbSpeedMultiplier); // climberSubsystem.updateHoldingPos(); } protected boolean isFinished() { return false; } protected void end() { - Robot.climberSubsystem.updateHoldingPos(); - Robot.climberSubsystem.setStiltsClimbSpeed(0); + Robot.stiltSubsystem.updateHoldingPos(); + Robot.stiltSubsystem.setStiltsClimbSpeed(0); } } \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveUpToLimit.java b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveUpToLimit.java new file mode 100644 index 0000000..ed5fa46 --- /dev/null +++ b/src/main/java/frc/team4909/robot/subsystems/stilts/commands/MoveUpToLimit.java @@ -0,0 +1,30 @@ +package frc.team4909.robot.subsystems.stilts.commands; + +import edu.wpi.first.wpilibj.command.Command; +import frc.team4909.robot.Robot; + +public class MoveUpToLimit extends Command { + + public MoveUpToLimit() { + requires(Robot.stiltSubsystem); + } + + @Override + protected void initialize() { + int maxAmps = 1; // @todo determine this value + Robot.stiltSubsystem.setCurrentLimit(maxAmps); + Robot.stiltSubsystem.setSpeed(.25); + } + + @Override + protected boolean isFinished() { + return Robot.stiltSubsystem.isAtTop(); + } + @Override + protected void end() { + Robot.stiltSubsystem.setSpeed(0); + Robot.stiltSubsystem.setCurrentLimit(0); //disable limit + Robot.stiltSubsystem.updateHoldingPos(); + Robot.stiltSubsystem.holdPosition(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/ElevatorArmSubsystem.java b/src/main/java/frc/team4909/robot/subsystems/wrist/WristSubsystem.java similarity index 84% rename from src/main/java/frc/team4909/robot/subsystems/elevatorarm/ElevatorArmSubsystem.java rename to src/main/java/frc/team4909/robot/subsystems/wrist/WristSubsystem.java index ef7f145..67ce1b5 100644 --- a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/ElevatorArmSubsystem.java +++ b/src/main/java/frc/team4909/robot/subsystems/wrist/WristSubsystem.java @@ -1,26 +1,21 @@ -package frc.team4909.robot.subsystems.elevatorarm; +package frc.team4909.robot.subsystems.wrist; -import frc.team4909.robot.Robot; import frc.team4909.robot.RobotConstants; import frc.team4909.robot.RobotMap; -import frc.team4909.robot.operator.controllers.BionicF310; -import frc.team4909.robot.subsystems.elevatorarm.commands.ElevatorArmOperatorControl; -import frc.team4909.robot.subsystems.elevatorarm.commands.SetAngle; -import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.team4909.robot.subsystems.wrist.commands.Default_WristOperatorControl; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -public class ElevatorArmSubsystem extends Subsystem{ +public class WristSubsystem extends Subsystem { TalonSRX wristMaster; - public int holdingPosition = 0; + private int holdingPosition = 0; - public ElevatorArmSubsystem(){ + public WristSubsystem(){ // super should always be called to ensure proper subystem initialization super(); // Wrist @@ -64,10 +59,6 @@ public ElevatorArmSubsystem(){ holdCurrentPosition(); } - public InstantCommand setAngle(int height){ - return new SetAngle(height, this); - } - public void holdCurrentPosition(){ holdingPosition = wristMaster.getSelectedSensorPosition(); } @@ -99,7 +90,15 @@ public void periodic() { @Override protected void initDefaultCommand() { - setDefaultCommand(new ElevatorArmOperatorControl()); + setDefaultCommand(new Default_WristOperatorControl()); + } + + public void updateHoldingPos() { + holdingPosition = getPosition(); + } + + public void updateHoldingPos(int pos) { + holdingPosition = pos; } diff --git a/src/main/java/frc/team4909/robot/subsystems/wrist/commands/Default_WristOperatorControl.java b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/Default_WristOperatorControl.java new file mode 100644 index 0000000..33ab1f4 --- /dev/null +++ b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/Default_WristOperatorControl.java @@ -0,0 +1,33 @@ +package frc.team4909.robot.subsystems.wrist.commands; + +import frc.team4909.robot.Robot; +import frc.team4909.robot.RobotConstants; +import frc.team4909.robot.operator.controllers.BionicF310; +import edu.wpi.first.wpilibj.command.Command; + +public class Default_WristOperatorControl extends Command { + + public Default_WristOperatorControl() { + requires(Robot.wristSubsystem); + } + + @Override + public void execute() { + // Sets speed to manipulator gamepad right Y stick value + double moveSpeed = -Robot.manipulatorGamepad.getThresholdAxis(BionicF310.RY) + * RobotConstants.elevatorArmSpeedMultiplier; + + if (moveSpeed == 0) { // If Y-stick value is not moving, HOLD position + Robot.wristSubsystem.updateHoldingPos(); + } else { // Set speed to Y-stick value and HOLD position + Robot.wristSubsystem.elevatorArmSetSpeed(moveSpeed); + Robot.wristSubsystem.updateHoldingPos(); + } + + } + + @Override + protected boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristAngle.java b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristAngle.java similarity index 71% rename from src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristAngle.java rename to src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristAngle.java index a27972a..93fc1d3 100644 --- a/src/main/java/frc/team4909/robot/subsystems/elevatorarm/commands/SetWristAngle.java +++ b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristAngle.java @@ -1,4 +1,4 @@ -package frc.team4909.robot.subsystems.elevatorarm.commands; +package frc.team4909.robot.subsystems.wrist.commands; import edu.wpi.first.wpilibj.command.InstantCommand; import frc.team4909.robot.Robot; @@ -12,7 +12,7 @@ public SetWristAngle(int setpoint) { @Override public void initialize() { - Robot.elevatorArmSubsystem.holdingPosition = setpoint; + Robot.wristSubsystem.updateHoldingPos(setpoint); } } \ No newline at end of file diff --git a/src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristSpeed.java b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristSpeed.java new file mode 100644 index 0000000..05f5e8c --- /dev/null +++ b/src/main/java/frc/team4909/robot/subsystems/wrist/commands/SetWristSpeed.java @@ -0,0 +1,30 @@ +package frc.team4909.robot.subsystems.wrist.commands; + +import edu.wpi.first.wpilibj.command.Command; +import frc.team4909.robot.Robot; + +public class SetWristSpeed extends Command { + + private double speed = 0; + + public SetWristSpeed(double speed1) { + requires(Robot.wristSubsystem); + speed = speed1; + } + + @Override + public void initialize() { + Robot.wristSubsystem.elevatorArmSetSpeed(speed); + } + @Override + protected void end() { + Robot.wristSubsystem.elevatorArmSetSpeed(0); + Robot.wristSubsystem.updateHoldingPos(); + } + + @Override + protected boolean isFinished() { + return false; + } + +} \ No newline at end of file