From c038c0607879f6e7c6d07c45d1ce6cefcd8da1d0 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 16 Jan 2024 17:08:38 -0500 Subject: [PATCH 1/2] Co-authored-by: jopy-man Co-authored-by: Zixi52 Co-authored-by: Lapcas Co-authored-by: chillmjs Co-authored-by: Ardian Agoes Co-authored-by: ambers7 Co-authored-by: Reya Miller --- .../com/stuypulse/robot/RobotContainer.java | 8 ++--- .../{ => elevator}/ElevatorDrive.java | 2 +- .../commands/elevator/ElevatorToAmp.java | 9 ++++++ .../{ => elevator}/ElevatorToBottom.java | 2 +- .../{ => elevator}/ElevatorToHeight.java | 2 +- .../{ => elevator}/ElevatorToTop.java | 3 +- .../commands/elevator/ElevatorToTrap.java | 10 +++++++ .../com/stuypulse/robot/constants/Ports.java | 3 +- .../stuypulse/robot/constants/Settings.java | 7 +++++ .../robot/subsystems/ElevatorImpl.java | 29 ++++++++++--------- 10 files changed, 51 insertions(+), 24 deletions(-) rename src/main/java/com/stuypulse/robot/commands/{ => elevator}/ElevatorDrive.java (95%) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToAmp.java rename src/main/java/com/stuypulse/robot/commands/{ => elevator}/ElevatorToBottom.java (80%) rename src/main/java/com/stuypulse/robot/commands/{ => elevator}/ElevatorToHeight.java (94%) rename src/main/java/com/stuypulse/robot/commands/{ => elevator}/ElevatorToTop.java (79%) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTrap.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f03a4c3..43e1683 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,11 +5,11 @@ package com.stuypulse.robot; -import com.stuypulse.robot.commands.ElevatorDrive; -import com.stuypulse.robot.commands.ElevatorToBottom; -import com.stuypulse.robot.commands.ElevatorToHeight; -import com.stuypulse.robot.commands.ElevatorToTop; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.elevator.ElevatorDrive; +import com.stuypulse.robot.commands.elevator.ElevatorToBottom; +import com.stuypulse.robot.commands.elevator.ElevatorToHeight; +import com.stuypulse.robot.commands.elevator.ElevatorToTop; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.Elevator; import com.stuypulse.stuylib.input.Gamepad; diff --git a/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorDrive.java similarity index 95% rename from src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java rename to src/main/java/com/stuypulse/robot/commands/elevator/ElevatorDrive.java index cf54b03..a8d19ca 100644 --- a/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorDrive.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.subsystems.Elevator; diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToAmp.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToAmp.java new file mode 100644 index 0000000..c138ae2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToAmp.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import static com.stuypulse.robot.constants.Settings.Elevator.*; + +public class ElevatorToAmp extends ElevatorToHeight { + public ElevatorToAmp() { + super(AMP_HEIGHT); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java similarity index 80% rename from src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java rename to src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java index f331329..5ad631b 100644 --- a/src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands; +package com.stuypulse.robot.commands.elevator; import static com.stuypulse.robot.constants.Settings.Elevator.*; diff --git a/src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java rename to src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java index 99e8abb..519895c 100644 --- a/src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands; +package com.stuypulse.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java similarity index 79% rename from src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java rename to src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java index 53a24a0..ffc43e8 100644 --- a/src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -1,8 +1,9 @@ -package com.stuypulse.robot.commands; +package com.stuypulse.robot.commands.elevator; import static com.stuypulse.robot.constants.Settings.Elevator.*; public class ElevatorToTop extends ElevatorToHeight { + public ElevatorToTop() { super(MAX_HEIGHT); } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTrap.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTrap.java new file mode 100644 index 0000000..6f9cd50 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTrap.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import static com.stuypulse.robot.constants.Settings.Elevator.*; + + +public class ElevatorToTrap extends ElevatorToHeight { + public ElevatorToTrap() { + super(TRAP_HEIGHT); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index c2b59d4..2197c68 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -14,8 +14,7 @@ public interface Gamepad { } public interface Elevator { - int LEFT = 0; - int RIGHT = 0; + int MOTOR = 0; int TOP_LIMIT_SWITCH = 0; int BOTTOM_LIMIT_SWITCH = 0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 36a38b9..8f81a1f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -7,6 +7,8 @@ import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.util.Units; + /*- * File containing tunable settings for every subsystem on the robot. * @@ -22,6 +24,8 @@ public interface Elevator { double MIN_HEIGHT = 0; double MAX_HEIGHT = 1.8475325; // meters + double TRAP_HEIGHT = 1.5; // placeholder + double AMP_HEIGHT = 1.25; // placeholder double MAX_HEIGHT_ERROR = 0.03; @@ -30,6 +34,9 @@ public interface Elevator { public interface Encoder { double ENCODER_MULTIPLIER = 1; + double GEAR_RATIO = 1.0 / 20.0; //added + double WINCH_CIRCUMFERENCE = Math.PI * Units.inchesToMeters(1.25); + double ENCODER_RATIO = GEAR_RATIO * WINCH_CIRCUMFERENCE; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/ElevatorImpl.java index f98fd65..20a0979 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ElevatorImpl.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings.Elevator.Encoder; import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.DigitalInput; @@ -15,21 +16,24 @@ public class ElevatorImpl extends Elevator { // hardware public CANSparkMax leftMotor; public CANSparkMax rightMotor; + public CANSparkMax motor; public RelativeEncoder leftEncoder; public RelativeEncoder rightEncoder; + public RelativeEncoder encoder; public DigitalInput topLimit; public DigitalInput bottomLimit; public ElevatorImpl() { + // motors - leftMotor = new CANSparkMax(Ports.Elevator.LEFT, MotorType.kBrushless); - rightMotor = new CANSparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless); + motor = new CANSparkMax(Ports.Elevator.MOTOR, MotorType.kBrushless); + + encoder = motor.getEncoder(); + encoder.setPositionConversionFactor(Encoder.ENCODER_RATIO); + encoder.setVelocityConversionFactor(Encoder.ENCODER_RATIO / 60.0); - // encoders - leftEncoder = leftMotor.getEncoder(); - rightEncoder = rightMotor.getEncoder(); // limit switches topLimit = new DigitalInput(Ports.Elevator.TOP_LIMIT_SWITCH); @@ -46,12 +50,12 @@ public boolean atBottom() { @Override public double getVelocity() { // average of the two? - return (rightEncoder.getVelocity() + leftEncoder.getVelocity()) / 2 * ENCODER_MULTIPLIER; + return encoder.getVelocity() * ENCODER_MULTIPLIER; } @Override public double getHeight() { - return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2 * ENCODER_MULTIPLIER; + return encoder.getPosition() * ENCODER_MULTIPLIER; } @Override @@ -61,19 +65,16 @@ public void setVoltage(double voltage) { voltage = 0.0; - leftEncoder.setPosition(MAX_HEIGHT); - rightEncoder.setPosition(MAX_HEIGHT); + encoder.setPosition(MAX_HEIGHT); } else if (atBottom() && voltage < 0) { DriverStation.reportWarning("Bottom Limit Reached", false); voltage = 0.0; - - leftEncoder.setPosition(MIN_HEIGHT); - rightEncoder.setPosition(MIN_HEIGHT); + + encoder.setPosition(MIN_HEIGHT); } - rightMotor.setVoltage(voltage); - leftMotor.setVoltage(voltage); + motor.setVoltage(voltage); } public void addTargetHeight(double delta) { From 0abb72757e7d647da6f176ab23e882ff66944882 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 16 Jan 2024 17:13:42 -0500 Subject: [PATCH 2/2] a --- src/main/java/com/stuypulse/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 43e1683..bf315f5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -67,8 +67,7 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Elevator To Bottom", new ElevatorToBottom().untilReady()); autonChooser.addOption("Elevator To Top", new ElevatorToTop().untilReady()); - autonChooser.addOption("Elevator To Height", new ElevatorToHeight(1.2).untilReady()); - + SmartDashboard.putData("Autonomous", autonChooser); }