Skip to content

Commit

Permalink
Merge pull request #45 from chopshop-166/armRotateImplementation
Browse files Browse the repository at this point in the history 
Added Arm Rotate map to ColdStart
  • Loading branch information
@msoucy
msoucy authored Feb 22, 2025
2 parents 8325974 + 288fd1b commit c108483
Show file tree
Hide file tree
Showing 9 changed files with 158 additions and 71 deletions.
25 changes: 16 additions & 9 deletions src/main/java/frc/robot/CommandSequences.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@ public class CommandSequences {
Elevator elevator;
ArmRotate armRotate;

public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralManip coralManip, Elevator elevator) {
public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralManip coralManip, Elevator elevator,
ArmRotate armRotate) {
this.drive = drive;
this.led = led;
this.algaeDestage = algaeDestage;
Expand All @@ -38,21 +39,27 @@ public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralMa
// Intakes until sensor is tripped, LEDs indicate that game piece is acquired

public Command intake() {
return armRotate.moveTo(ArmRotatePresets.INTAKE).andThen(led.elevatorToPreset(),
return armRotate.moveIntaking().andThen(led.elevatorToPreset(),
elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
armRotate.moveTo(ArmRotatePresets.INTAKE),
led.intaking(), coralManip.intake(), led.gamePieceAcquired());
}

public Command intakeBottom() {
return armRotate.moveTo(ArmRotatePresets.INTAKE).alongWith(coralManip.intake());
}

// Moves elevator to set coral preset

public Command moveElevator(ElevatorPresets level) {
return led.elevatorToPreset().andThen(elevator.moveTo(level), led.elevatorAtPreset());
public Command moveElevator(ElevatorPresets level, ArmRotatePresets preset) {
return led.elevatorToPreset().andThen(armRotate.moveTo(ArmRotatePresets.OUT), elevator.moveTo(level),
led.elevatorAtPreset(), armRotate.moveTo(preset));
}

// Scores on set coral preset, then stows elevator

public Command score() {
return armRotate.moveTo(ArmRotatePresets.SCOREL23).andThen(coralManip.score(), (led.elevatorToPreset()),
public Command score(ArmRotatePresets preset) {
return coralManip.score().andThen((led.elevatorToPreset()),
elevator.moveTo(ElevatorPresets.STOW),
led.elevatorAtPreset());
}
Expand All @@ -76,15 +83,15 @@ public Command scoreL1Auto() {
}

public Command positionL2Auto() {
return moveElevator(ElevatorPresets.SCOREL2);
return moveElevator(ElevatorPresets.SCOREL2, ArmRotatePresets.SCOREL2);
}

public Command positionL3Auto() {
return moveElevator(ElevatorPresets.SCOREL3);
return moveElevator(ElevatorPresets.SCOREL3, ArmRotatePresets.SCOREL3);
}

public Command positionL4Auto() {
return moveElevator(ElevatorPresets.SCOREL4);
return moveElevator(ElevatorPresets.SCOREL4, ArmRotatePresets.SCOREL4);
}

// Scores on L1 preset, then stows elevator
Expand Down
62 changes: 42 additions & 20 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,12 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.maps.RobotMap;
import frc.robot.maps.subsystems.ArmRotateMap.ArmRotatePresets;
import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
import frc.robot.subsystems.AlgaeDestage;
import frc.robot.subsystems.ArmRotate;
import frc.robot.subsystems.CoralManip;
import frc.robot.subsystems.DeepClimb;
import frc.robot.subsystems.Drive;
Expand All @@ -37,6 +40,7 @@ public final class Robot extends CommandRobot {
private RobotMap map = getRobotMap(RobotMap.class, new RobotMap());
private ButtonXboxController driveController = new ButtonXboxController(0);
private ButtonXboxController copilotController = new ButtonXboxController(1);
private Trigger elevatorSafeTrigger;

// Helpers
final DoubleUnaryOperator driveScaler = getScaler(0.45, 0.25);
Expand All @@ -54,8 +58,10 @@ public final class Robot extends CommandRobot {
private CoralManip coralManip = new CoralManip(map.getCoralManipMap());
private Elevator elevator = new Elevator(map.getElevatorMap());
private DeepClimb deepClimb = new DeepClimb(map.getDeepClimbMap());
private ArmRotate armRotate = new ArmRotate(map.getArmRotateMap());

private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator);
private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator,
armRotate);

NetworkTableInstance ntinst = NetworkTableInstance.getDefault();

Expand All @@ -70,7 +76,8 @@ public void registerNamedCommands() {
NamedCommands.registerCommand("Score Coral L2", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL2));
NamedCommands.registerCommand("Score Coral L3", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL3));
NamedCommands.registerCommand("Score Coral L4", commandSequences.scoreL4());
NamedCommands.registerCommand("Stow", commandSequences.moveElevator(ElevatorPresets.STOW));
NamedCommands.registerCommand("Stow",
commandSequences.moveElevator(ElevatorPresets.STOW, ArmRotatePresets.STOW));
NamedCommands.registerCommand("Zero Da Elevatah", elevator.zero());
}

Expand All @@ -83,6 +90,7 @@ public Robot() {
super();
registerNamedCommands();
autoChooser = AutoBuilder.buildAutoChooser();
elevatorSafeTrigger = new Trigger(() -> elevator.elevatorSafeTrigger().getAsBoolean());
}

@Override
Expand Down Expand Up @@ -135,29 +143,39 @@ public void configureButtonBindings() {
.whileTrue(drive.robotCentricDrive());

copilotController.a().onTrue(commandSequences.intake());

copilotController.x().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1))
.onFalse(commandSequences.scoreL1());

copilotController.b().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2))
.onFalse(commandSequences.score());

copilotController.y().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3))
.onFalse(commandSequences.score());
copilotController.leftBumper().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4))
.onFalse(commandSequences.scoreL4());
elevatorSafeTrigger.onTrue(commandSequences.intakeBottom());

copilotController.x()
.whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1, ArmRotatePresets.SCOREL1))
.onFalse(coralManip.score());

copilotController.b()
.whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2, ArmRotatePresets.SCOREL2))
.onFalse(coralManip.score());

copilotController.y()
.whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3, ArmRotatePresets.SCOREL3))
.onFalse(coralManip.score());
// copilotController.leftBumper()
// .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4,
// ArmRotatePresets.SCOREL4))
// .onFalse(commandSequences.scoreL4());
copilotController.back().onTrue(elevator.safeStateCmd());
copilotController.start().onTrue(elevator.zero());
copilotController.getPovButton(POVDirection.RIGHT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL2))
.onFalse(elevator.safeStateCmd());
copilotController.getPovButton(POVDirection.UP).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL3))
.onFalse(elevator.safeStateCmd());
copilotController.getPovButton(POVDirection.LEFT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL1))
.onFalse(elevator.safeStateCmd());
copilotController.getPovButton(POVDirection.DOWN).whileTrue(elevator.moveTo(ElevatorPresets.STOW))
copilotController.getPovButton(POVDirection.LEFT).whileTrue(elevator.moveTo(ElevatorPresets.STOW))
.onFalse(elevator.safeStateCmd());

copilotController.rightBumper().whileTrue(coralManip.feed());
// copilotController.getPovButton(POVDirection.DOWN).whileTrue(elevator.moveTo(ElevatorPresets.STOW))
// .onFalse(elevator.safeStateCmd());
copilotController.getPovButton(POVDirection.DOWN).whileTrue(coralManip.feed());

copilotController.rightBumper().whileTrue(armRotate.moveTo(ArmRotatePresets.OUT));
copilotController.leftBumper()
.whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4, ArmRotatePresets.SCOREL4))
.onFalse(coralManip.score());
}

@Override
Expand All @@ -178,8 +196,12 @@ public Command getAutoCommand() {
@Override
public void setDefaultCommands() {
elevator.setDefaultCommand(elevator.move(RobotUtils.deadbandAxis(.1, () -> -copilotController.getLeftY())));
deepClimb
.setDefaultCommand(deepClimb.rotate(RobotUtils.deadbandAxis(0.1, () -> copilotController.getRightY())));
armRotate.setDefaultCommand(armRotate.move(RobotUtils.deadbandAxis(.1, () -> -copilotController.getRightY())));

// deepClimb
// .setDefaultCommand(deepClimb.rotate(RobotUtils.deadbandAxis(0.1, () ->
// copilotController.getRightY())));

}

public DoubleUnaryOperator getScaler(double leftRange, double rightRange) {
Expand Down
38 changes: 29 additions & 9 deletions src/main/java/frc/robot/maps/ColdStart.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
import com.chopshop166.chopshoplib.motors.CSSparkFlex;
import com.chopshop166.chopshoplib.motors.CSSparkMax;
import com.chopshop166.chopshoplib.motors.SmartMotorControllerGroup;
import com.chopshop166.chopshoplib.sensors.CSEncoder;
import com.chopshop166.chopshoplib.sensors.IEncoder;
import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2;
import com.chopshop166.chopshoplib.states.PIDValues;
import com.pathplanner.lib.config.ModuleConfig;
Expand All @@ -24,15 +26,18 @@
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogEncoder;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import frc.robot.maps.subsystems.ArmRotateMap;
import frc.robot.maps.subsystems.CoralManipMap;
import frc.robot.maps.subsystems.DeepClimbMap;
import frc.robot.maps.subsystems.ElevatorMap;
Expand Down Expand Up @@ -148,25 +153,40 @@ public ElevatorMap getElevatorMap() {
elevatorMotors.validateEncoderRate(.2, 10);
return new ElevatorMap(
elevatorMotors, leftMotor.getEncoder(),
new ElevatorMap.ElevatorPresetValues(16.6, 5, 14, 29.5, 56, 57.5, 1),
new ValueRange(0, 57.5), new ValueRange(6, 53), pid, feedForward);
new ElevatorMap.ElevatorPresetValues(0, 15, 19.5, 34.5, 57.5, 57.5, 1),
new ValueRange(0, 58.25), new ValueRange(6, 53), pid, feedForward);
}

@Override
public ArmRotateMap getArmRotateMap() {
CSSparkFlex motor = new CSSparkFlex(10);
DutyCycleEncoder absEncoder = new DutyCycleEncoder(0, 360, 212);
SparkFlexConfig config = new SparkFlexConfig();

config.smartCurrentLimit(30);
config.idleMode(IdleMode.kBrake);
config.inverted(true);
config.voltageCompensation(11.5);
motor.getMotorController().configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
ProfiledPIDController pid = new ProfiledPIDController(0.025, 0, 0, new Constraints(90, 300));
pid.setTolerance(1);
ArmFeedforward feedForward = new ArmFeedforward(0.04, 0.0, 0.0018);
return new ArmRotateMap(motor, absEncoder,
new ArmRotateMap.ArmRotatePresetValues(96.3, 66, 66, 66, 66, 66, 96.3), pid,
new ValueRange(5, 97), new ValueRange(0, 94), feedForward);
}

@Override
public CoralManipMap getCoralManipMap() {
CSSparkMax leftWheels = new CSSparkMax(9);
CSSparkMax rightWheels = new CSSparkMax(10);
CSSparkMax motor = new CSSparkMax(9);
SparkMaxConfig config = new SparkMaxConfig();
config.smartCurrentLimit(30);
config.idleMode(IdleMode.kBrake);
leftWheels.getMotorController().configure(config, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
// Right is identical to left, but inverted
config.inverted(true);
rightWheels.getMotorController().configure(config, ResetMode.kResetSafeParameters,
motor.getMotorController().configure(config, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
CSDigitalInput sensor = new CSDigitalInput(9);
return new CoralManipMap(leftWheels, rightWheels, sensor::get);
return new CoralManipMap(motor, sensor::get);
}

@Override
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/maps/RobotMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import com.chopshop166.chopshoplib.maps.VisionMap;

import frc.robot.maps.subsystems.AlgaeDestageMap;
import frc.robot.maps.subsystems.ArmRotateMap;
import frc.robot.maps.subsystems.CoralManipMap;
import frc.robot.maps.subsystems.DeepClimbMap;
import frc.robot.maps.subsystems.ElevatorMap;
Expand Down Expand Up @@ -50,6 +51,10 @@ public DeepClimbMap getDeepClimbMap() {
return new DeepClimbMap();
}

public ArmRotateMap getArmRotateMap() {
return new ArmRotateMap();
}

public void setupLogging() {
// Pull the replay log from AdvantageScope (or prompt the user)
String logPath = LogFileUtil.findReplayLog();
Expand Down
41 changes: 31 additions & 10 deletions src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,13 @@
import com.chopshop166.chopshoplib.logging.LoggableMap;
import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
import com.chopshop166.chopshoplib.motors.SmartMotorController;
import com.chopshop166.chopshoplib.sensors.IAbsolutePosition;
import com.chopshop166.chopshoplib.sensors.IEncoder;
import com.chopshop166.chopshoplib.sensors.MockEncoder;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.math.controller.ArmFeedforward;

public class ArmRotateMap implements LoggableMap<ArmRotateMap.Data> {
Expand All @@ -20,19 +24,25 @@ public enum ArmRotatePresets {

SCOREL1,

SCOREL23,
SCOREL2,

SCOREL3,

SCOREL4,

OUT,

STOW,

HOLD

}

public record ArmRotatePresetValues(double intake, double scoreL1, double scoreL23, double scoreL4, double stow) {
public record ArmRotatePresetValues(double intake, double scoreL1, double scoreL2, double scoreL3, double scoreL4,
double out,
double stow) {
public ArmRotatePresetValues() {
this(0, 0, 0, 0, 0);
this(0, 0, 0, 0, 0, 0, 0);
}

public double getValue(ArmRotatePresets preset) {
Expand All @@ -43,10 +53,14 @@ public double getValue(ArmRotatePresets preset) {
return intake;
case SCOREL1:
return scoreL1;
case SCOREL23:
return scoreL23;
case SCOREL2:
return scoreL2;
case SCOREL3:
return scoreL3;
case SCOREL4:
return scoreL4;
case OUT:
return out;
case STOW:
return stow;
default:
Expand All @@ -56,14 +70,21 @@ public double getValue(ArmRotatePresets preset) {
}

public SmartMotorController motor;
public final IEncoder encoder;
public final DutyCycleEncoder encoder;
public final ArmRotatePresetValues armRotatePreset;
public final ProfiledPIDController pid;
public final ValueRange hardLimits;
public final ValueRange softLimits;
public final ArmFeedforward armFeedforward;

public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePresetValues armRotatePreset,
public ArmRotateMap() {
this(new SmartMotorController(), new DutyCycleEncoder(0), new ArmRotatePresetValues(),
new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)), new ValueRange(0, 0), new ValueRange(0, 0),
new ArmFeedforward(0, 0, 0));

}

public ArmRotateMap(SmartMotorController motor, DutyCycleEncoder encoder, ArmRotatePresetValues armRotatePreset,
ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) {
this.motor = motor;
this.encoder = encoder;
Expand All @@ -77,13 +98,13 @@ public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePrese
@Override
public void updateData(Data data) {
data.motor.updateData(motor);
data.rotationAbsAngleDegrees = encoder.getAbsolutePosition();
data.rotatingAngleVelocity = encoder.getRate();
data.rotationVelocity = (data.rotationAbsAngleDegrees - encoder.get()) / .02;
data.rotationAbsAngleDegrees = encoder.get();
}

public static class Data extends DataWrapper {
public MotorControllerData motor = new MotorControllerData();
public double rotationAbsAngleDegrees;
public double rotatingAngleVelocity;
public double rotationVelocity;
}
}
Loading

0 comments on commit c108483

Please sign in to comment.