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

March 25 #92

Merged
merged 9 commits into from
Mar 26, 2024
Merged

March 25 #92

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
1290e2e
Revert "Add back feeder IR sensor (#89)"
Mar 25, 2024
996c737
Merge branch 'main' of https://github.com/StuyPulse/Izzi into se/marc…
Mar 25, 2024
0756950
Tune for voltage
Mar 25, 2024
8242cf7
Apply 65amp current limit
Mar 25, 2024
583eb99
Remove voltage zero at bottom lift
Mar 25, 2024
0325872
Invert feeder motor
Mar 25, 2024
1ce07e8
Use normal turn kP for assist kP
Mar 25, 2024
1502f07
Ferry shot changes
Mar 25, 2024
0c56659
Reorganize ferry command
BenG49 Mar 26, 2024
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
10 changes: 2 additions & 8 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -154,10 +154,8 @@ private void configureDriverBindings() {
// score speaker no align
driver.getRightMenuButton()
.onTrue(new ShooterPodiumShot())
.whileTrue(new ConveyorToShooter()
.andThen(new WaitCommand(Settings.Conveyor.AT_FEEDER_WAIT_DELAY.get()))
.andThen(new ShooterWaitForTarget()
.withTimeout(1.5))
.onTrue(new ShooterWaitForTarget()
.withTimeout(1.5)
.andThen(new ConveyorShoot()))
.onFalse(new ConveyorStop())
.onFalse(new IntakeStop());
Expand Down Expand Up @@ -254,10 +252,6 @@ private void configureOperatorBindings() {
.onFalse(new ConveyorStop())
.onFalse(new IntakeStop())
.onFalse(new AmperStop());
operator.getRightBumper()
.onTrue(new ConveyorToShooter())
.onFalse(new ConveyorStop())
.onFalse(new IntakeStop());

operator.getTopButton()
.onTrue(new AmperScore())
Expand Down
4 changes: 1 addition & 3 deletions src/main/java/com/stuypulse/robot/commands/auton/ADE/ThreePieceADE.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
Expand Down Expand Up @@ -33,8 +32,7 @@ public ThreePieceADE(PathPlannerPath... paths) {
// intake A
new FollowPathAndIntake(paths[0]),
// shoot A
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

// intake D
Expand Down
4 changes: 1 addition & 3 deletions src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
Expand Down Expand Up @@ -32,8 +31,7 @@ public BlayFivePieceCBAE(PathPlannerPath... paths) {
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[1]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[2]),
Expand Down
7 changes: 2 additions & 5 deletions src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAE.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
Expand Down Expand Up @@ -35,13 +34,11 @@ public FivePieceCBAE(PathPlannerPath... paths) {
new FollowPathAlignAndShoot(paths[1], new SwerveDriveToShoot()),

new FollowPathAndIntake(paths[2]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[3]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[4]),
Expand Down
10 changes: 3 additions & 7 deletions src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumCBAE.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
Expand Down Expand Up @@ -32,18 +31,15 @@ public FivePiecePodiumCBAE(PathPlannerPath... paths) {
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[0]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[1]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[2]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[3]),
Expand Down
7 changes: 2 additions & 5 deletions src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumForwardCBAE.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
Expand All @@ -31,13 +30,11 @@ public FivePiecePodiumForwardCBAE(PathPlannerPath... paths) {
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[0]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[1]),
new SwerveDriveToShoot()
.alongWith(new ConveyorToShooter()),
new SwerveDriveToShoot(),
new ConveyorShootRoutine(),

new FollowPathAndIntake(paths[2]),
Expand Down
3 changes: 0 additions & 3 deletions src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
import com.stuypulse.robot.commands.conveyor.ConveyorStop;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget;
Expand All @@ -28,8 +27,6 @@ public double getPathTime(PathPlannerPath path) {
public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) {
addCommands(
new ParallelCommandGroup(
new ConveyorToShooter()
.withTimeout(3.0),
SwerveDrive.getInstance().followPathCommand(path),
new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE)
.andThen(new ShooterPodiumShot())
Expand Down
3 changes: 0 additions & 3 deletions src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingAlignAndShoot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import com.pathplanner.lib.path.PathPlannerPath;
import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
import com.stuypulse.robot.commands.conveyor.ConveyorStop;
import com.stuypulse.robot.commands.conveyor.ConveyorToShooter;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget;
Expand All @@ -27,8 +26,6 @@ public double getPathTime(PathPlannerPath path) {
public FollowPathTrackingAlignAndShoot(PathPlannerPath path, Command alignCommand) {
addCommands(
new ParallelCommandGroup(
new ConveyorToShooter()
.withTimeout(3.0),
SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path),
new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE)
.andThen(new ShooterPodiumShot())
Expand Down
1 change: 0 additions & 1 deletion src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@ public ConveyorShootRoutine() {
public ConveyorShootRoutine(double delay) {
addCommands(
new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()),
new ConveyorToShooter(),
new ConveyorShoot(),
new WaitCommand(delay),
new ConveyorStop(),
Expand Down
42 changes: 0 additions & 42 deletions src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java
View file
Open in desktop

This file was deleted.

108 changes: 90 additions & 18 deletions src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,60 +7,132 @@
import com.stuypulse.robot.subsystems.odometry.Odometry;
import com.stuypulse.robot.subsystems.shooter.Shooter;
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
import com.stuypulse.stuylib.control.angle.AngleController;
import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
import com.stuypulse.stuylib.input.Gamepad;

import com.stuypulse.stuylib.math.Angle;
import com.stuypulse.stuylib.streams.numbers.IStream;
import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
import com.stuypulse.stuylib.streams.vectors.VStream;
import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
import com.stuypulse.stuylib.util.AngleVelocity;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.Driver.Drive;
import com.stuypulse.robot.constants.Settings.Swerve.Assist;

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.wpilibj2.command.Command;


public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned {
public class SwerveDriveAutoFerry extends Command {

private final SwerveDrive swerve;
private final Odometry odometry;
private final Shooter shooter;
private final Conveyor conveyor;
private final Intake intake;

private final VStream drive;

public SwerveDriveAutoFerry(Gamepad driver) {
super(driver);
private final AngleController controller;
private final IStream angleVelocity;

public SwerveDriveAutoFerry(Gamepad driver) {
swerve = SwerveDrive.getInstance();
shooter = Shooter.getInstance();
odometry = Odometry.getInstance();
conveyor = Conveyor.getInstance();
intake = Intake.getInstance();

addRequirements(shooter, odometry);
drive = VStream.create(driver::getLeftStick)
.filtered(
new VDeadZone(Drive.DEADBAND),
x -> x.clamp(1),
x -> x.pow(Drive.POWER.get()),
x -> x.mul(Drive.MAX_TELEOP_SPEED.get()),
new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()),
new VLowPassFilter(Drive.RC.get()));

controller = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD)
.setOutputFilter(x -> -x);

AngleVelocity derivative = new AngleVelocity();

angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle())))
.filtered(new LowPassFilter(Assist.ANGLE_DERIV_RC))
// make angleVelocity contribute less once distance is less than REDUCED_FF_DIST
// so that angular velocity doesn't oscillate
.filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST))
.filtered(x -> -x);


addRequirements(swerve, shooter, odometry, conveyor, intake);
}

// returns pose of close amp corner
private Translation2d getTargetPose() {
return Robot.isBlue()
? new Translation2d(0, Field.WIDTH)
: new Translation2d(0, 0);
? new Translation2d(0, Field.WIDTH - 0.5)
: new Translation2d(0, 0.5);
}

@Override
public Rotation2d getTargetAngle() {
return getTargetPose().minus(odometry.getPose().getTranslation()).getAngle();
private Rotation2d getTargetAngle() {
return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle();
}

private double getDistanceToTarget() {
return odometry.getPose().getTranslation().getDistance(getTargetPose());
}

private boolean shouldMoveBack() {
Translation2d robot = odometry.getPose().getTranslation();

return robot.getX() < Field.FERRY_SHOT_MIN_FAR_X
&& robot.getY() < Field.WIDTH / 2.0 + 1;
}

private boolean canShoot() {
Translation2d robot = odometry.getPose().getTranslation();

if (shouldMoveBack())
return false;

if (robot.getX() > Field.FERRY_SHOT_MAX_X)
return false;

return true;
}

@Override
public double getDistanceToTarget() {
return getTargetPose().getDistance(odometry.getPose().getTranslation());
public void initialize() {
shooter.setTargetSpeeds(Settings.Shooter.FERRY);
}

@Override
public void execute() {
super.execute();
double omega = angleVelocity.get() + controller.update(
Angle.fromRotation2d(getTargetAngle()),
Angle.fromRotation2d(odometry.getPose().getRotation()));

if (shouldMoveBack()) {
swerve.setFieldRelativeSpeeds(new ChassisSpeeds(1, 0, 0));

if (odometry.getPose().getX() < Field.FERRY_SHOT_THRESHOLD_X) {
intake.acquire();
conveyor.toShooter();
shooter.setTargetSpeeds(Settings.Shooter.FERRY);
} else {
intake.stop();
conveyor.stop();
} else {
if (canShoot() && Math.abs(controller.getError().toDegrees()) < Assist.FERRY_ALIGN_THRESHOLD_DEG) {
intake.acquire();
conveyor.toShooter();
} else {
intake.stop();
conveyor.stop();
}

swerve.drive(drive.get(), omega);
}
}

Expand Down
3 changes: 2 additions & 1 deletion src/main/java/com/stuypulse/robot/constants/Field.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,8 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria

/*** FERRYING ***/

double FERRY_SHOT_THRESHOLD_X = 9.0;
double FERRY_SHOT_MAX_X = 9.0;
double FERRY_SHOT_MIN_FAR_X = 8.25;

/**** EMPTY FIELD POSES ****/

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/constants/Motors.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ public interface Shooter {

public interface Conveyor {
CANSparkConfig GANDALF_MOTOR = new CANSparkConfig(true, IdleMode.kBrake,500, 0.25);
CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 500, 0.1);
CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.1);
}

public interface Climber {
Expand Down
Loading
Loading