Bradley/SendableDemo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding an expanded Elevator subsystem and adjusting the supporting players

Browse Source
This commit is contained in:
Bradley Bickford 2024-11-24 21:40:35 -05:00
parent 45fe78a10e
commit f1667bfbe3
4 changed files with 119 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/main/java/frc/robot/RobotContainer.java
View File

@ -4,32 +4,73 @@
package frc.robot;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.OIConstants;
import frc.robot.constants.ElevatorConstants.ElevatorPositions;
import frc.robot.subsystems.Elevator;
public class RobotContainer {
private Elevator elevator;
private CommandXboxController driver;
public RobotContainer() {
elevator = new Elevator();
driver = new CommandXboxController(OIConstants.kDriverUSB);
configureBindings();
configureShuffleboard();
}
private void configureBindings() {
elevator.setDefaultCommand(
elevator.debugSetpoints()
elevator.assistedManualControl(driver::getRightY)
);
driver.a().onTrue(elevator.goToPosition(ElevatorPositions.kBottom, 3));
driver.b().onTrue(elevator.goToPosition(ElevatorPositions.kLowMid, 3));
driver.x().onTrue(elevator.goToPosition(ElevatorPositions.kHighMid, 3));
driver.y().onTrue(elevator.goToPosition(ElevatorPositions.kTop, 3));
}
private void configureShuffleboard() {
ShuffleboardTab tab = Shuffleboard.getTab("MyTab");
tab.add("Elevator Subsystem", elevator);
tab.addDouble("Elevator Encoder Distance", elevator::getEncoderDistance)
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
tab.addDouble("Elevator Encoder Rate", elevator::getEncoderRate)
.withPosition(2, 1)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
tab.addDouble("Elevator Bottom Setpoint", ElevatorPositions.kBottom::getPositionValue)
.withPosition(0, 1)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
tab.addDouble("Elevator Low-Mid Setpoint", ElevatorPositions.kLowMid::getPositionValue)
.withPosition(2, 1)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
tab.addDouble("Elevator High-Mid Setpoint", ElevatorPositions.kHighMid::getPositionValue)
.withPosition(0, 2)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
tab.addDouble("Elevator Top Setpoint", ElevatorPositions.kTop::getPositionValue)
.withPosition(2, 2)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
}
public Command getAutonomousCommand() {

27
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -1,13 +1,33 @@
package frc.robot.constants;
public interface ElevatorConstants {
public enum ElevatorPositions {
kBottom(0),
kLowMid(1),
kHighMid(2),
kTop(3);
private double position;
private ElevatorPositions(double position) {
this.position = position;
}
public double getPositionValue() {
return position;
}
public void setPositionValue(double newPosition) {
position = newPosition;
}
}
public static final int kMotor1CANID = 0;
public static final int kMotor2CANID = 1;
public static final int kEncoderChannelADIO = 0;
public static final int kEncoderChannelBDIO = 1;
public static final double kEncoderDistancePerPulse = 0;
public static final double kEncoderDistancePerPulse = 0.0000000001; //Avoids UncleanStatusException distancPerPulse must not be 0
public static final double kPPositional = 0;
public static final double kIPositional = 0;
@ -24,8 +44,5 @@ public interface ElevatorConstants {
public static final double kFFG = 0;
public static final double kFFV = 0;
public static final double kDefaultBottomPositionalSetpoint = 0;
public static final double kDefaultLowMidPositionalSetpoint = 1;
public static final double kDefaultHighMidPositionalSetpoint = 2;
public static final double kDefaultTopPositionalSetpoint = 3;
public static final double kElevatorMaxVelocity = 0;
}

5
src/main/java/frc/robot/constants/OIConstants.java Normal file
View File

@ -0,0 +1,5 @@
package frc.robot.constants;
public class OIConstants {
public static final int kDriverUSB = 0;
}

75
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -1,5 +1,7 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import edu.wpi.first.math.controller.ElevatorFeedforward;
@ -7,11 +9,11 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RepeatCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.ElevatorConstants.ElevatorPositions;
public class Elevator extends SubsystemBase {
private WPI_VictorSPX motor1;
@ -24,16 +26,14 @@ public class Elevator extends SubsystemBase {
private ElevatorFeedforward eff;
private double currentBottomPositionalSetpoint;
private double currentLowMidPositionalSetpoint;
private double currentHighMidPositionalSetpoint;
private double currentTopPositionalSetpoint;
public Elevator() {
motor1 = new WPI_VictorSPX(ElevatorConstants.kMotor1CANID);
motor2 = new WPI_VictorSPX(ElevatorConstants.kMotor2CANID);
motor2.follow(motor1);
encoder = new Encoder(ElevatorConstants.kEncoderChannelADIO, ElevatorConstants.kEncoderChannelBDIO);
encoder.setDistancePerPulse(ElevatorConstants.kEncoderDistancePerPulse);
positionalController = new PIDController(
ElevatorConstants.kPPositional,
@ -52,20 +52,47 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kFFG,
ElevatorConstants.kFFV
);
}
currentBottomPositionalSetpoint = ElevatorConstants.kDefaultBottomPositionalSetpoint;
currentLowMidPositionalSetpoint = ElevatorConstants.kDefaultLowMidPositionalSetpoint;
currentHighMidPositionalSetpoint = ElevatorConstants.kDefaultHighMidPositionalSetpoint;
currentTopPositionalSetpoint = ElevatorConstants.kDefaultTopPositionalSetpoint;
public double getEncoderDistance() {
return encoder.getDistance();
}
public double getEncoderRate() {
return encoder.getRate();
}
public Command goToPosition(ElevatorPositions position, double timeout) {
return run(() -> {
double voltsOut = positionalController.calculate(
encoder.getDistance(),
position.getPositionValue()
) + eff.calculate(0);
motor1.setVoltage(voltsOut);
}).until(positionalController::atSetpoint).withTimeout(timeout);
}
public Command assistedManualControl(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
double voltsOut = velocityController.calculate(
encoder.getRate(),
realSpeedTarget
) + eff.calculate(realSpeedTarget);
motor1.setVoltage(voltsOut);
});
}
public Command debugSetpoints() {
return new RepeatCommand(
runOnce(() -> {
System.out.println("Current Bottom Positional Setpoint: " + currentBottomPositionalSetpoint);
System.out.println("Current Low-Mid Positional Setpoint: " + currentLowMidPositionalSetpoint);
System.out.println("Current High-Mid Positional Setpoint: " + currentHighMidPositionalSetpoint);
System.out.println("Current Top Positional Setpoint: " + currentTopPositionalSetpoint);
System.out.println("Current Bottom Positional Setpoint: " + ElevatorPositions.kBottom.getPositionValue());
System.out.println("Current Low-Mid Positional Setpoint: " + ElevatorPositions.kLowMid.getPositionValue());
System.out.println("Current High-Mid Positional Setpoint: " + ElevatorPositions.kHighMid.getPositionValue());
System.out.println("Current Top Positional Setpoint: " + ElevatorPositions.kTop.getPositionValue());
System.out.println("");
}).andThen(new WaitCommand(5))
);
@ -77,30 +104,26 @@ public class Elevator extends SubsystemBase {
builder.addDoubleProperty(
".currentBottomPositionalSetpoint",
() -> currentBottomPositionalSetpoint,
(d) -> {
System.out.println("Current Bottom Positional Setpoint Changed!");
System.out.println("Old Value: " + currentBottomPositionalSetpoint + " New Value: " + d);
currentBottomPositionalSetpoint = d;
}
() -> ElevatorPositions.kBottom.getPositionValue(),
(d) -> ElevatorPositions.kBottom.setPositionValue(d)
);
builder.addDoubleProperty(
".currentLowMidPositionalSetpoint",
() -> currentLowMidPositionalSetpoint,
(d) -> currentLowMidPositionalSetpoint = d
() -> ElevatorPositions.kLowMid.getPositionValue(),
(d) -> ElevatorPositions.kLowMid.setPositionValue(d)
);
builder.addDoubleProperty(
".currentHighMidPositionalSetpoint",
() -> currentHighMidPositionalSetpoint,
(d) -> currentHighMidPositionalSetpoint = d
() -> ElevatorPositions.kHighMid.getPositionValue(),
(d) -> ElevatorPositions.kHighMid.setPositionValue(d)
);
builder.addDoubleProperty(
".currentTopPositionalSetpoint",
() -> currentTopPositionalSetpoint,
(d) -> currentTopPositionalSetpoint = d
() -> ElevatorPositions.kTop.getPositionValue(),
(d) -> ElevatorPositions.kTop.setPositionValue(d)
);
}
}