Skip to content

Commit

Permalink
8/28 (#35)
Browse files Browse the repository at this point in the history 
* fix logging

* enable voltage compensation to 12V for cansparkmax motor controllers

* burn flash after voltage compensation

* update led instructions, add voltage compensation setting for motor configs

* forgot to push changes to LEDDefaultMode

* add attention button, dont apply anti recoil voltage if past max arm angle
  • Loading branch information
@IanShiii
IanShiii authored Aug 28, 2024
1 parent 5c09508 commit 87c6920
Show file tree
Hide file tree
Showing 6 changed files with 51 additions and 72 deletions.
36 changes: 10 additions & 26 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,15 +131,15 @@ private void configureDriverBindings() {
.onTrue(new ArmToFeed())
// .whileTrue(new SwerveDriveDriveToNote(driver))
.whileTrue(new IntakeAcquire()
.deadlineWith(new LEDSet(LEDInstructions.INTAKING))
.deadlineWith(new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING))
.andThen(new BuzzController(driver))
);

// intake robot relative
driver.getLeftTriggerButton()
.onTrue(new ArmToFeed())
.whileTrue(new IntakeAcquire()
.deadlineWith(new LEDSet(LEDInstructions.INTAKING))
.deadlineWith(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING))
.andThen(new BuzzController(driver))
)
.whileTrue(new SwerveDriveDriveRobotRelative(driver));
Expand All @@ -155,7 +155,7 @@ private void configureDriverBindings() {
.whileTrue(new ConditionalCommand(
new ArmWaitUntilAtTarget()
.withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.andThen(new ShooterFeederDeacquire()),
.andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))),
new SwerveDriveDriveAlignedSpeaker(driver)
.alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER))
.andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
Expand All @@ -165,10 +165,7 @@ private void configureDriverBindings() {
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
.until(() -> swerve.isAlignedToSpeaker())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
),
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)),
() -> Arm.getInstance().getState() == Arm.State.AMP))
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -186,10 +183,7 @@ private void configureDriverBindings() {
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLobFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN))
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -208,10 +202,7 @@ private void configureDriverBindings() {
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLowFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN))
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand Down Expand Up @@ -247,10 +238,7 @@ private void configureDriverBindings() {
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLobFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL))
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -268,19 +256,15 @@ private void configureDriverBindings() {
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLowFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL))
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// climbing
driver.getRightButton().onTrue(new ArmToPreClimb());
driver.getBottomButton().onTrue(new ArmToClimbing());
// human player attention button
driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION));
}

private void configureOperatorBindings() {
Expand Down
11 changes: 2 additions & 9 deletions src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,8 @@ public LEDDefaultMode() {

private LEDInstruction getInstruction() {

switch (arm.getState()) {
case PRE_CLIMB:
return LEDInstructions.ARM_PRECLIMB;
case CLIMBING:
return LEDInstructions.ARM_POSTCLIMB;
case AMP:
return LEDInstructions.AMP_WITHOUT_ALIGN;
default:
break;
if (Arm.getInstance().getState() == Arm.State.AMP) {
return LEDInstructions.ARM_AT_AMP;
}

if (intake.hasNote() || shooter.hasNote()) {
Expand Down
26 changes: 11 additions & 15 deletions src/main/java/com/stuypulse/robot/constants/LEDInstructions.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,29 +55,25 @@ public interface LEDInstructions {

LEDInstruction DEFAULT = LEDInstructions.OFF;

LEDInstruction INTAKING = new LEDPulseColor(SLColor.BLUE);
LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.RED);
LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow();
LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE);
LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD);

LEDInstruction SPEAKER_ALIGN = GREEN;
LEDInstruction SPEAKER_MANUAL = new LEDPulseColor(SLColor.GREEN);
LEDInstruction SPEAKER_MANUAL = new LEDRainbow();

LEDInstruction LOW_FERRY_ALIGN = LIME;
LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.LIME);
LEDInstruction LOW_FERRY_ALIGN = PURPLE;
LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.PURPLE);

LEDInstruction LOB_FERRY_ALIGN = AQUA;
LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.AQUA);
LEDInstruction LOB_FERRY_ALIGN = GREEN;
LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.GREEN);

LEDInstruction AMP_WITH_ALIGN = VIOLET;
LEDInstruction AMP_WITHOUT_ALIGN = new LEDPulseColor(SLColor.VIOLET);

LEDInstruction IS_ALIGNED = RAINBOW;
LEDInstruction ARM_AT_AMP = YELLOW;
LEDInstruction AMP_SCORE = GREEN;

LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE);

LEDInstruction ARM_PRECLIMB = ORANGE;
LEDInstruction ARM_POSTCLIMB = new LEDPulseColor(SLColor.ORANGE);

LEDInstruction CONTAINS_NOTE = GOLD;
LEDInstruction CONTAINS_NOTE = RED;

// TO FUTURE USERS, DONT PUT LEDAlign and LEDAutonChooser (any disabled LEDInstructions) inside
// the LEDInstructions interface
Expand Down
35 changes: 22 additions & 13 deletions src/main/java/com/stuypulse/robot/constants/Motors.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,20 +39,20 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) {
/** Classes to store all of the values a motor needs */

public interface Arm {
CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25);
CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25);
CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25, false);
CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, false);
}

public interface Intake {
CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25);
CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25);
CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25);
CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25, true);
CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true);
CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true);
}

public interface Shooter {
CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5);
CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5);
CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25);
CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5, true);
CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5, true);
CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, true);
}

/* Configurations */
Expand Down Expand Up @@ -91,31 +91,37 @@ public static class CANSparkConfig {
public final IdleMode IDLE_MODE;
public final int CURRENT_LIMIT_AMPS;
public final double OPEN_LOOP_RAMP_RATE;
public final boolean ENABLE_VOLTAGE_COMPENSATION;

public CANSparkConfig(
boolean inverted,
IdleMode idleMode,
int currentLimitAmps,
double openLoopRampRate) {
double openLoopRampRate,
boolean enableVoltageCompensation) {
this.INVERTED = inverted;
this.IDLE_MODE = idleMode;
this.CURRENT_LIMIT_AMPS = currentLimitAmps;
this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
this.ENABLE_VOLTAGE_COMPENSATION = enableVoltageCompensation;
}

public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
this(inverted, idleMode, currentLimitAmps, 0.0);
public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps, boolean enableVoltageCompensation) {
this(inverted, idleMode, currentLimitAmps, 0.0, enableVoltageCompensation);
}

public CANSparkConfig(boolean inverted, IdleMode idleMode) {
this(inverted, idleMode, 80);
public CANSparkConfig(boolean inverted, IdleMode idleMode, boolean enableVoltageCompensation) {
this(inverted, idleMode, 80, enableVoltageCompensation);
}

public void configure(CANSparkBase motor) {
motor.setInverted(INVERTED);
motor.setIdleMode(IDLE_MODE);
motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
if (ENABLE_VOLTAGE_COMPENSATION) {
motor.enableVoltageCompensation(12);
}
motor.burnFlash();
}

Expand All @@ -124,6 +130,9 @@ public void configureAsFollower(CANSparkMax motor, CANSparkMax follows) {
motor.setIdleMode(IDLE_MODE);
motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
if (ENABLE_VOLTAGE_COMPENSATION) {
motor.enableVoltageCompensation(12);
}
motor.follow(follows);
motor.burnFlash();
}
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,9 +71,7 @@ public boolean atTarget() {
if (state == State.FEED) {
return atValidFeedAngle();
}
boolean atTarget = Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get();
SmartDashboard.putBoolean("test/armAtTarget", atTarget);
return atTarget;
return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get();
}

@Override
Expand Down Expand Up @@ -184,7 +182,7 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger
}
else {
controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees());
if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) {
if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE.get()) {
setVoltage(controller.getOutput() + 0.31);
}
else {
Expand All @@ -211,6 +209,8 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger
SmartDashboard.putNumber("Arm/Right Duty Cycle", rightMotor.get());

SmartDashboard.putNumber("Arm/Arm Angle", getDegrees());
SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin)
SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin)]

SmartDashboard.putBoolean("Arm/At Target", atTarget());
}
}
5 changes: 1 addition & 4 deletions src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,10 +171,7 @@ public boolean isAlignedToSpeaker() {
Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation();
// Rotate by 180 because the shooter is on the back of the robot
Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle().rotateBy(Rotation2d.fromDegrees(180));
SmartDashboard.putNumber("test/angleError", getPose().getRotation().minus(targetAngle).getDegrees());
boolean isAligned = Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get();
SmartDashboard.putBoolean("test/isAligned", isAligned);
return isAligned;
return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get();
}

public boolean isAlignedToLowFerry() {
Expand Down

0 comments on commit 87c6920

Please sign in to comment.