Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added Arm Rotate map to ColdStart #45

Merged
merged 6 commits into from
Feb 22, 2025
Merged

Added Arm Rotate map to ColdStart #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
aa77e0c
Removed second motor from coral manipulator
Liammurray19 Feb 20, 2025
3935ac7
added arm rotate map into ColdStarts map
Liammurray19 Feb 20, 2025
6ca8b45
Uncommented arm rotate in coldStart, fixed duty cycle encoder
Liammurray19 Feb 20, 2025
8468a62
PID on arm rotate some command sequences tweaked
Liammurray19 Feb 22, 2025
1bce566
Robot scores on all levels with command sequences
Liammurray19 Feb 22, 2025
288fd1b
Elevator moves to intake much faster
Liammurray19 Feb 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 16 additions & 9 deletions src/main/java/frc/robot/CommandSequences.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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