Skip to content

Commit

Permalink
Side shoot autos
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichaelLesirge
MichaelLesirge committed Mar 16, 2024
1 parent 504847e commit 238a168
Show file tree
Hide file tree
Showing 6 changed files with 150 additions and 32 deletions.
51 changes: 45 additions & 6 deletions shuffleboard.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
"_showGlyph": true
}
},
"8,3": {
"8,2": {
"size": [
2,
1
Expand All @@ -37,9 +37,9 @@
"_showGlyph": true
}
},
"8,2": {
"0,3": {
"size": [
2,
4,
1
],
"content": {
Expand Down Expand Up @@ -80,7 +80,7 @@
"_showGlyph": true
}
},
"5,3": {
"6,2": {
"size": [
1,
1
Expand Down Expand Up @@ -306,7 +306,7 @@
"Colors/Color when false": "#8B0000FF"
}
},
"5,2": {
"5,3": {
"size": [
1,
1
Expand All @@ -319,7 +319,7 @@
"_showGlyph": false
}
},
"6,2": {
"6,3": {
"size": [
1,
1
Expand All @@ -331,6 +331,45 @@
"_glyph": 148,
"_showGlyph": false
}
},
"9,3": {
"size": [
1,
1
],
"content": {
"_type": "Command",
"_source0": "network_table:///SmartDashboard/ZeroYaw",
"_title": "ZeroYaw",
"_glyph": 139,
"_showGlyph": true
}
},
"8,3": {
"size": [
1,
1
],
"content": {
"_type": "Command",
"_source0": "network_table:///SmartDashboard/ArmUp",
"_title": "ArmUp",
"_glyph": 484,
"_showGlyph": true
}
},
"5,2": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/FlywheelTime",
"_title": "FlywheelTime",
"_glyph": 148,
"_showGlyph": false
}
}
}
}
Expand Down
1 change: 1 addition & 0 deletions simgui-ds.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,7 @@
}
],
"robotJoysticks": [
{},
{
"guid": "Keyboard0"
}
Expand Down
4 changes: 3 additions & 1 deletion simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,10 @@
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/ArmUp": "Command",
"/SmartDashboard/Auto Chooser": "String Chooser",
"/SmartDashboard/SendableChooser[0]": "String Chooser"
"/SmartDashboard/SendableChooser[0]": "String Chooser",
"/SmartDashboard/ZeroYaw": "Command"
}
},
"NetworkTables": {
Expand Down
52 changes: 35 additions & 17 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,20 +30,24 @@
import frc.robot.subsystems.intake.IntakeShooter;
import frc.robot.subsystems.intake.RealShooter;

import java.util.Optional;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.GenericHID.HIDType;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

Expand Down Expand Up @@ -135,20 +139,19 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
autoChooser.setDefaultOption("Forward", Autos.startingAuto(drivetrain, arm, leftHang, rightHang));
autoChooser.addOption("1+Forward",
Autos.shootStartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
autoChooser.addOption("2+Forward",
autoChooser.addOption("Forward", Autos.startingAuto(drivetrain, arm, leftHang, rightHang));
autoChooser.setDefaultOption("2+Forward",
Autos.shoot2StartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
autoChooser.addOption("2+AmpSide",
Autos.shootFromAmpSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
autoChooser.addOption("2+SourceSide",
Autos.shootFromFarSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));

SmartDashboard.putData("Auto Chooser", autoChooser);

SmartDashboard.putData("ArmUp", new ArmRotateTo(arm, ArmConstants.ARM_START_DEGREES));
SmartDashboard.putData("ZeroYaw", new InstantCommand(drivetrain::zeroYaw));

SmartDashboard.putData(drivetrain);
SmartDashboard.putData(arm);
SmartDashboard.putData(intakeShooter);

SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);

configureBindings();
Expand Down Expand Up @@ -184,6 +187,11 @@ public void setUpDriveController() {
new InstantCommand(drivetrain::toDefaultStates, drivetrain));
new InstantCommand(lightStrip::toDefaultPattern, lightStrip);

double flip = 1;
Optional<Alliance> ally = DriverStation.getAlliance();
if (ally.isPresent() && ally.get() == Alliance.Red)
flip = -1;

if (genericHIDType == null) {
SmartDashboard.putString("Drive Ctrl", onPortMsg + "None");
} else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
Expand Down Expand Up @@ -230,10 +238,12 @@ public void setUpDriveController() {

xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative));

// xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true));
// xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
// xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain,
// Rotation2d.fromDegrees(0), true));
// xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain,
// Rotation2d.fromDegrees(-90), true));
xbox.rightBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0)));
xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90)));
xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90 * flip)));

if (vision != null)
xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision));
Expand Down Expand Up @@ -277,12 +287,19 @@ public void setUpOperatorController() {
joystick.button(1).onTrue(Autos.intakeFromFloorStart(arm, intakeShooter));
joystick.button(1).onFalse(Autos.intakeFromFloorEnd(arm, intakeShooter));

joystick.button(2).onTrue(Autos.shootInSpeaker(drivetrain, arm, intakeShooter, vision, inputs));
joystick.button(2).onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs).withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
joystick.button(3).onTrue(Autos.shootInSpeaker(drivetrain, arm, intakeShooter, vision, inputs).withInterruptBehavior(InterruptionBehavior.kCancelIncoming));

joystick.button(4).onTrue(new SpinFlywheelShooter(intakeShooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER));
joystick.button(4).onFalse(new SpinFlywheelShooter(intakeShooter, 0));

joystick.button(5).onTrue(stowArm);
joystick.button(6).onTrue(stowArm2);

joystick.button(3).onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs));
joystick.button(7).whileTrue(Commands.startEnd(intakeShooter::eject, intakeShooter::stop, intakeShooter));

joystick.button(4).onTrue(stowArm);
joystick.button(6).onTrue(stowArm);
leftHang.setDefaultCommand(new HangControl(leftHang, joystick::getX));
rightHang.setDefaultCommand(new HangControl(rightHang, joystick::getY));

joystick.button(10).onTrue(cancelCommand);

Expand All @@ -299,7 +316,8 @@ public void setUpOperatorController() {

xbox.rightBumper().onTrue(
new SpinFlywheelShooter(intakeShooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER));
xbox.rightBumper().onFalse(new SpinFlywheelShooter(intakeShooter, 0));
xbox.rightBumper().onFalse(
new SpinFlywheelShooter(intakeShooter, 0).withInterruptBehavior(InterruptionBehavior.kCancelSelf));

xbox.y().onTrue(stowArm);
xbox.a().onTrue(stowArm2);
Expand Down
56 changes: 53 additions & 3 deletions src/main/java/frc/robot/commands/Autos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,55 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I
new PullHangerDown(leftHang, HangConstants.SPEED));
}

public static Command shootFromSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Hang leftHang,
Hang rightHang, boolean flipped) {

Optional<Alliance> ally = DriverStation.getAlliance();
if (ally.isPresent() && ally.get() == Alliance.Red) {
flipped = !flipped;
}

drivetrain.setFrontOffset(Rotation2d.fromDegrees(flipped ? -60 : 60));

return Commands.parallel(
Commands.sequence(
// First note
shootInSpeaker(drivetrain, arm, shooter, null, null),

new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES),
// Line up X to second note
new AutoDriveTo(drivetrain, new Translation2d(0.25, 0)),
new AutoRotateTo(drivetrain, new Rotation2d(0), true),

// Pick up First note
intakeFromFloorStart(arm, shooter),
new AutoDriveTo(drivetrain, new Translation2d(2.15, 0)),
intakeFromFloorEnd(arm, shooter),

new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES),
// Get Back
new AutoDriveTo(drivetrain, new Translation2d(-2.15, 0)),
new AutoRotateTo(drivetrain, new Rotation2d(flipped ? 60 : -60), true),
new AutoDriveTo(drivetrain, new Translation2d(0.25, 0)),

// Shoot Second
shootInSpeaker(drivetrain, arm, shooter, null, null)
),

new PullHangerDown(rightHang, HangConstants.SPEED),
new PullHangerDown(leftHang, HangConstants.SPEED));
}

public static Command shootFromAmpSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter,
Hang leftHang, Hang rightHang) {
return shootFromSideAuto(drivetrain, arm, shooter, leftHang, rightHang, false);
}

public static Command shootFromFarSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter,
Hang leftHang, Hang rightHang) {
return shootFromSideAuto(drivetrain, arm, shooter, leftHang, rightHang, true);
}

public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Vision vision,
ChassisDriveInputs inputs) {

Expand All @@ -114,9 +163,10 @@ public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShoo
}

return Commands.sequence(
new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES).alongWith(
new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP),
new WaitCommand(0.5)),
Commands.parallel(
new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP,
0.5),
new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES)),
new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_AMP),
new WaitCommand(0.2),
new SpinFlywheelShooter(shooter, 0),
Expand Down
18 changes: 13 additions & 5 deletions src/main/java/frc/robot/subsystems/intake/RealShooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.revrobotics.CANSparkLowLevel;
import com.revrobotics.CANSparkMax;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants.IntakeShooterConstants;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
Expand Down Expand Up @@ -48,8 +49,11 @@ public RealShooter(int flywheel1Id, int flywheel2Id, int intakeID, int intakeLim
@Override
public void setFlyWheelShooterSpeed(double speed) {
SmartDashboard.putNumber("FlywheelShooter", speed);
flywheelSpeed = speed;
timeFlywheelSet = System.currentTimeMillis();

if (flywheelSpeed != speed) {
flywheelSpeed = speed;
timeFlywheelSet = System.currentTimeMillis();
}

flywheel1.set(-speed);
flywheel2.set(-speed);
Expand All @@ -76,9 +80,8 @@ public void eject() {

@Override
public void stop() {
flywheel1.set(0);
flywheel2.set(0);
intake.set(0);
setFlyWheelShooterSpeed(0);
setIntakeGrabberSpeed(0);
flywheel1.stopMotor();
flywheel2.stopMotor();
intake.stopMotor();
Expand All @@ -90,4 +93,9 @@ public double howLongAtSpeedMillis(double speed) {
return -1;
return System.currentTimeMillis() - timeFlywheelSet;
}

@Override
public void periodic() {
SmartDashboard.putString("FlywheelTime", flywheelSpeed + " for " + Units.millisecondsToSeconds(System.currentTimeMillis() - timeFlywheelSet));
}
}

0 comments on commit 238a168

Please sign in to comment.