Skip to content

Commit

Permalink
[Robot] Clean up shooter + calculate optimal idle angle
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmheuer
rmheuer committed Feb 27, 2024
1 parent f80a65e commit 232ca44
Show file tree
Hide file tree
Showing 8 changed files with 153 additions and 100 deletions.
2 changes: 1 addition & 1 deletion Robot/src/main/java/com/swrobotics/robot/config/NTData.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.swrobotics.lib.net.NTBoolean;
import com.swrobotics.lib.net.NTDouble;
import com.swrobotics.lib.net.NTEntry;
import edu.wpi.first.networktables.NetworkTableInstance;

public final class NTData {
// GOOD
Expand Down Expand Up @@ -96,7 +97,6 @@ public final class NTData {
public static final NTEntry<Double> SHOOTER_FLYWHEEL_KV = new NTDouble("Shooter/Flywheel/PIDV/kV", 0.126).setPersistent();

// GOOD (except for allowable pct err)
public static final NTEntry<Double> SHOOTER_PIVOT_IDLE_ANGLE = new NTDouble("Shooter/Pivot/Idle Angle (deg)", 35).setPersistent();
public static final NTEntry<Double> SHOOTER_PIVOT_KP = new NTDouble("Shooter/Pivot/PID/kP", 600).setPersistent();
public static final NTEntry<Double> SHOOTER_PIVOT_KD = new NTDouble("Shooter/Pivot/PID/kD", 0).setPersistent();
public static final NTEntry<Double> SHOOTER_PIVOT_KV = new NTDouble("Shooter/Pivot/PID/kV", 0).setPersistent();
Expand Down
97 changes: 55 additions & 42 deletions Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,11 @@
package com.swrobotics.robot.control;

import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import com.swrobotics.lib.input.XboxController;
import com.swrobotics.mathlib.MathUtil;
import com.swrobotics.robot.RobotContainer;

import com.swrobotics.robot.commands.AimTowardsSpeakerCommand;
import com.swrobotics.robot.commands.ShootCommand;
import com.swrobotics.robot.config.NTData;
import com.swrobotics.robot.subsystems.amp.AmpArmSubsystem;
import com.swrobotics.robot.subsystems.amp.AmpIntakeSubsystem;
Expand All @@ -17,13 +14,10 @@
import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem;
import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;

Expand Down Expand Up @@ -126,16 +120,7 @@ private boolean getRobotRelativeDrive() {
return driver.leftBumper.isPressed();
}

@Override
public void periodic() {
if (!DriverStation.isTeleop()) {
climberState = ClimberArm.State.RETRACTED_IDLE;
robot.indexer.setReverse(false);
robot.intake.setReverse(false);
robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop
return;
}

private void drivePeriodic() {
Translation2d translation = getDriveTranslation();
if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) {
translation = translation.rotateBy(Rotation2d.fromDegrees(180));
Expand All @@ -145,10 +130,10 @@ public void periodic() {
Rotation2d rotation = new Rotation2d(MathUtil.TAU * rawRotation);

ChassisSpeeds chassisRequest = ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(),
translation.getY(),
rotation.getRadians(),
robot.drive.getEstimatedPose().getRotation());
translation.getX(),
translation.getY(),
rotation.getRadians(),
robot.drive.getEstimatedPose().getRotation());

if (getRobotRelativeDrive()) {
chassisRequest = new ChassisSpeeds(-translation.getX(), -translation.getY(), rotation.getRadians());
Expand All @@ -158,16 +143,20 @@ public void periodic() {
SwerveDrive.DRIVER_PRIORITY,
chassisRequest,
DriveRequestType.Velocity);
}

IntakeSubsystem.State intakeState = IntakeSubsystem.State.OFF;
if (operator.a.isPressed())
intakeState = IntakeSubsystem.State.INTAKE;

// Indexer uses the intake state also
robot.intake.set(intakeState);
boolean operatorWantsShoot = operator.b.isPressed();
robot.indexer.setFeedToShooter(operatorWantsShoot);
private void climberPeriodic() {
if (operator.rightBumper.isRising()) {
boolean extended = climberState == ClimberArm.State.EXTENDED;
climberState = extended ? ClimberArm.State.RETRACTED_IDLE : ClimberArm.State.EXTENDED;
}
if (operator.rightTrigger.isOutside(TRIGGER_BUTTON_THRESHOLD)) {
climberState = ClimberArm.State.RETRACTED_HOLD;
}
robot.climber.setState(climberState);
}

private void ampArmPeriodic() {
AmpArmSubsystem.Position ampArmPosition = AmpArmSubsystem.Position.STOW;
AmpIntakeSubsystem.State ampIntakeState = AmpIntakeSubsystem.State.OFF;
if (operator.x.isPressed()) {
Expand All @@ -183,31 +172,55 @@ public void periodic() {
}
robot.ampArm.setPosition(ampArmPosition);
robot.ampIntake.setState(ampIntakeState);
}

if (operator.rightBumper.isRising()) {
boolean extended = climberState == ClimberArm.State.EXTENDED;
climberState = extended ? ClimberArm.State.RETRACTED_IDLE : ClimberArm.State.EXTENDED;
}
if (operator.rightTrigger.isOutside(TRIGGER_BUTTON_THRESHOLD)) {
climberState = ClimberArm.State.RETRACTED_HOLD;
}
robot.climber.setState(climberState);
private void intakePeriodic() {
IntakeSubsystem.State intakeState = IntakeSubsystem.State.OFF;
if (operator.a.isPressed())
intakeState = IntakeSubsystem.State.INTAKE;

// Indexer uses the intake state also (feeds in when intake is on)
robot.intake.set(intakeState);
robot.intake.setReverse(operator.back.isPressed());
}

private void shooterPeriodic() {
boolean aimAtSpeaker = driverWantsAim();

boolean shoot = operator.b.isPressed();
robot.indexer.setFeedToShooter(shoot);
robot.indexer.setReverse(operator.start.isPressed());

boolean shootAmp = operator.y.isPressed();
if (shootAmp)
robot.shooter.setTempAimCalculator(AmpAimCalculator.INSTANCE);
boolean aimAtAmp = operator.y.isPressed();
if (aimAtAmp)
robot.shooter.setTempAimCalculator(AmpAimCalculator.INSTANCE); // TODO: Probably better way to do this

boolean runFlywheelForShooting = aimAtSpeaker || aimAtAmp || driverWantsFlywheels();

// robot.shooter.setFlywheelControl(driverWantsAim() || driverWantsFlywheels());
ShooterSubsystem.FlywheelControl flywheelControl = ShooterSubsystem.FlywheelControl.IDLE;
if (operator.start.isPressed())
flywheelControl = ShooterSubsystem.FlywheelControl.REVERSE;
else if (driverWantsAim() || driverWantsFlywheels() || shootAmp)
else if (runFlywheelForShooting)
flywheelControl = ShooterSubsystem.FlywheelControl.SHOOT;
else if (operatorWantsShoot)
else if (shoot)
flywheelControl = ShooterSubsystem.FlywheelControl.POOP;
robot.shooter.setFlywheelControl(flywheelControl);
}

@Override
public void periodic() {
if (!DriverStation.isTeleop()) {
climberState = ClimberArm.State.RETRACTED_IDLE;
robot.indexer.setReverse(false);
robot.intake.setReverse(false);
robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop
return;
}

drivePeriodic();
climberPeriodic();
// ampArmPeriodic();
intakePeriodic();
shooterPeriodic();
}
}
2 changes: 1 addition & 1 deletion Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public void periodic() {
prideSequencer.apply(this);
} else if (robot.climber.getCurrentState() == ClimberArm.State.RETRACTED_HOLD) {
showClimberHold();
} else if (robot.shooter.isPreparing()) {
} else if (robot.shooter.isPreparingToShoot()) {
showShooterStatus();
} else if (robot.drive.getLastSelectedPriority() == SwerveDrive.AUTO_PRIORITY) {
showAutoDriving();
Expand Down
30 changes: 18 additions & 12 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,17 @@
import com.swrobotics.robot.config.IOAllocation;
import com.swrobotics.robot.config.NTData;
import com.swrobotics.robot.logging.SimView;
import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator;
import com.swrobotics.robot.utils.TalonFXWithSim;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public final class PivotSubsystem extends SubsystemBase {
private enum State {
CALIBRATING,
IDLE,
OFF,
SHOOTING
}

Expand Down Expand Up @@ -62,40 +65,42 @@ public PivotSubsystem() {
setpoint = Double.NEGATIVE_INFINITY;
}

public void setTargetAngle(double angleRot) {
if (state == State.CALIBRATING)
return;

private void setTargetAngle(double angleRot) {
angleRot = MathUtil.clamp(
angleRot,
hardStopAngle + 2 / 360.0,
maxAngle - 2 / 360.0);

motor.setControl(new PositionVoltage(angleRot));
state = State.SHOOTING;
setpoint = angleRot;
}

public void setShooting(double angleRot) {
if (state == State.CALIBRATING)
return;

setTargetAngle(angleRot);
state = State.SHOOTING;
}

public void setIdle() {
if (state == State.CALIBRATING)
return;

if (state != State.IDLE)
// Don't recalibrate in auto to save time
if (state != State.IDLE && !DriverStation.isAutonomous())
calibrated = false;

setTargetAngle(NTData.SHOOTER_PIVOT_IDLE_ANGLE.get() / 360.0);
setTargetAngle(TableAimCalculator.INSTANCE.getIdleAngle() / MathUtil.TAU);
state = State.IDLE;
}

public void setNeutral() {
if (state == State.CALIBRATING)
return;

if (state != State.IDLE)
calibrated = false;

motor.setControl(new NeutralOut());
state = State.IDLE;
state = State.OFF;
}

@Override
Expand All @@ -106,13 +111,14 @@ public void periodic() {
calibrated = false;
}

if (state != State.SHOOTING && !calibrated) {
if ((state == State.CALIBRATING || state == State.IDLE) && !calibrated) {
motor.setControl(new VoltageOut(-NTData.SHOOTER_PIVOT_CALIBRATE_VOLTS.get()));
limitSwitch.refresh();
boolean atLimit = limitSwitch.getValue() == ReverseLimitValue.ClosedToGround;

if (atLimit) {
motor.setPosition(hardStopAngle);
motor.setControl(new NeutralOut());

calibrated = true;
if (state == State.CALIBRATING)
Expand Down
Loading

0 comments on commit 232ca44

Please sign in to comment.