Skip to content
This repository has been archived by the owner on Jul 13, 2023. It is now read-only.

Commit

Permalink
San Diego Regional fixes (#307)
Browse files Browse the repository at this point in the history 
* auto fix and preset adding

* fixes

* limelight port forwarding

* auto shooter rpm tweak (it worked)

Co-authored-by: IanSzalai <[email protected]>
  • Loading branch information
@KaneBarnes @IanSzalai
KaneBarnes and IanSzalai authored Mar 23, 2022
1 parent 67e791a commit 62c74cb
Show file tree
Hide file tree
Showing 10 changed files with 222 additions and 148 deletions.
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import com.frcteam3255.preferences.SN_Preferences;

import edu.wpi.first.util.net.PortForwarder;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand Down Expand Up @@ -38,6 +39,9 @@ public void robotInit() {
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
DriverStation.silenceJoystickConnectionWarning(true);
PortForwarder.add(5800, "limelight.local", 5800);
PortForwarder.add(5801, "limelight.local", 5801);
PortForwarder.add(5805, "limelight.local", 5805);

}

Expand Down
81 changes: 50 additions & 31 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,12 @@
import frc.robot.commands.Intake.*;
import frc.robot.commands.Shooter.*;
import frc.robot.commands.Transfer.*;
import frc.robot.RobotPreferences.DrivetrainPrefs;
// import frc.robot.RobotPreferences.DrivetrainPrefs;
import frc.robot.RobotPreferences.HoodPrefs;
import frc.robot.RobotPreferences.ShooterPrefs;
import frc.robot.RobotPreferences.TurretPrefs;
import frc.robot.commands.ConfigureSubsystems;
import frc.robot.commands.Autonomous.DriveDistanceOpenLoop;
// import frc.robot.commands.Autonomous.DriveDistanceOpenLoop;
import frc.robot.commands.Autonomous.OpenLoopTwoBall;
import frc.robot.commands.Climber.*;
import frc.robot.subsystems.*;
Expand Down Expand Up @@ -60,45 +60,59 @@ public class RobotContainer {

// Drivetrain Commands
private final Drive com_drive = new Drive(sub_drivetrain);
private final DriveDistanceOpenLoop com_driveOpenLoop = new DriveDistanceOpenLoop(
sub_drivetrain, DrivetrainPrefs.driveOpenLoopCounts, DrivetrainPrefs.driveOpenLoopSpeedForward);
// private final DriveDistanceOpenLoop com_driveOpenLoop = new
// DriveDistanceOpenLoop(
// sub_drivetrain, DrivetrainPrefs.driveOpenLoopCounts,
// DrivetrainPrefs.driveOpenLoopSpeedForward);

// Hood Commands
private final ShallowHood com_shallowHood = new ShallowHood(sub_hood);
private final SteepenHood com_steepenHood = new SteepenHood(sub_hood);

private final DriveMotionProfile com_driveTestPath = new DriveMotionProfile(sub_drivetrain,
"testpath_left.csv", "testpath_right.csv");
private final DriveMotionProfile com_drive2020Field = new DriveMotionProfile(sub_drivetrain,
"full2020path_left.csv", "full2020path_right.csv");
private final DriveMotionProfile com_driveHanger = new DriveMotionProfile(sub_drivetrain,
"hanger_left.csv", "hanger_right.csv");
// private final DriveMotionProfile com_driveTestPath = new
// DriveMotionProfile(sub_drivetrain,
// "testpath_left.csv", "testpath_right.csv");
// private final DriveMotionProfile com_drive2020Field = new
// DriveMotionProfile(sub_drivetrain,
// // "full2020path_left.csv", "full2020path_right.csv");
// // private final DriveMotionProfile com_driveHanger = new
// DriveMotionProfile(sub_drivetrain,
// "hanger_left.csv", "hanger_right.csv");

// Turret Commands
private final MoveTurret com_moveTurret = new MoveTurret(sub_turret);
private final SetTurretPosition com_setTurretCenter = new SetTurretPosition(sub_turret,
RobotPreferences.zeroDoublePref);
private final SetTurretPosition com_setTurretPos1 = new SetTurretPosition(sub_turret,
RobotPreferences.TurretPrefs.turretPresetPos1);
private final HoldTurretPosition com_holdTurretCenter = new HoldTurretPosition(sub_turret, sub_navX,
RobotPreferences.zeroDoublePref);
private final HoldTurretPosition com_holdTurretPos1 = new HoldTurretPosition(sub_turret, sub_navX,
RobotPreferences.TurretPrefs.turretPresetPos1);
// private final SetTurretPosition com_setTurretPos1 = new
// SetTurretPosition(sub_turret,
// RobotPreferences.TurretPrefs.turretPresetPos1);
// private final HoldTurretPosition com_holdTurretCenter = new
// HoldTurretPosition(sub_turret, sub_navX,
// RobotPreferences.zeroDoublePref);
// private final HoldTurretPosition com_holdTurretPos1 = new
// HoldTurretPosition(sub_turret, sub_navX,
// RobotPreferences.TurretPrefs.turretPresetPos1);
private final VisionAimTurret com_visionAimTurret = new VisionAimTurret(sub_turret, sub_shooter, sub_vision);
private final VisionNavXAimTurret com_visionHoldAimTurret = new VisionNavXAimTurret(sub_turret, sub_vision,
sub_navX);
// private final VisionNavXAimTurret com_visionHoldAimTurret = new
// VisionNavXAimTurret(sub_turret, sub_vision,
// sub_navX);

// Shooter Commands
private final PushCargoToShooter com_pushCargoToShooter = new PushCargoToShooter(sub_shooter, sub_transfer);
private final PushCargoWithDelay com_pushCargoWithDelay = new PushCargoWithDelay(sub_shooter, sub_transfer);
// private final PushCargoToShooter com_pushCargoToShooter = new
// PushCargoToShooter(sub_shooter, sub_transfer);
// private final PushCargoWithDelay com_pushCargoWithDelay = new
// PushCargoWithDelay(sub_shooter, sub_transfer);
private final PushCargoSimple com_pushCargoSimple = new PushCargoSimple(sub_shooter, sub_transfer);
private final SpinFlywheelVelocity com_spinFlywheelVelocity = new SpinFlywheelVelocity(sub_shooter);
private final SpinFlywheelPercentOutput com_FlywheelPercentOutput = new SpinFlywheelPercentOutput(
sub_shooter);
// private final SpinFlywheelVelocity com_spinFlywheelVelocity = new
// SpinFlywheelVelocity(sub_shooter);
// private final SpinFlywheelPercentOutput com_FlywheelPercentOutput = new
// SpinFlywheelPercentOutput(
// sub_shooter);
private final SpinFlywheelGoalRPM com_spinFlywheelGoalRPM = new SpinFlywheelGoalRPM(sub_shooter);

private final InstantCommand com_setUpperHubGoal = new InstantCommand(sub_shooter::setGoalUpperHub);
private final InstantCommand com_setLowerHubGoal = new InstantCommand(sub_shooter::setGoalLowerHub);
// private final InstantCommand com_setLowerHubGoal = new
// InstantCommand(sub_shooter::setGoalLowerHub);

// Shooter Presets
private final PresetShooter com_presetFender = new PresetShooter(sub_shooter, sub_hood,
Expand All @@ -110,9 +124,12 @@ public class RobotContainer {
private final PresetShooter com_presetLaunchpadUpper = new PresetShooter(sub_shooter, sub_hood,
ShooterPrefs.shooterPresetUpperLaunchpadRPM, HoodPrefs.hoodPresetUpperLaunchpadSteep,
ShooterPrefs.shooterPresetLowerLaunchpadRPM, HoodPrefs.hoodPresetLowerLaunchpadSteep);
private final PresetShooter com_presetTerminalUpper = new PresetShooter(sub_shooter, sub_hood,
ShooterPrefs.shooterPresetUpperTerminalRPM, HoodPrefs.hoodPresetUpperTerminalSteep,
ShooterPrefs.shooterPresetLowerTerminalRPM, HoodPrefs.hoodPresetLowerTerminalSteep);
// private final PresetShooter com_presetTerminalUpper = new
// PresetShooter(sub_shooter, sub_hood,
// ShooterPrefs.shooterPresetUpperTerminalRPM,
// HoodPrefs.hoodPresetUpperTerminalSteep,
// ShooterPrefs.shooterPresetLowerTerminalRPM,
// HoodPrefs.hoodPresetLowerTerminalSteep);

private final SetTurretPosition com_presetForward = new SetTurretPosition(sub_turret, TurretPrefs.turretSnapForwards);
private final SetTurretPosition com_presetBackward = new SetTurretPosition(sub_turret,
Expand All @@ -131,9 +148,9 @@ public class RobotContainer {
private final SetGoalRPM com_setGoalRPM = new SetGoalRPM(sub_shooter, sub_vision);

// Climber Commands
private final Climb com_climb = new Climb(sub_climber);
// private final Climb com_climb = new Climb(sub_climber);
private final MagicClimb com_magicClimb = new MagicClimb(sub_climber);
private final ResetClimber com_resetClimber = new ResetClimber(sub_climber);
// private final ResetClimber com_resetClimber = new ResetClimber(sub_climber);
private final InstantCommand com_lockClimber = new InstantCommand(sub_climber::lockClimber);
private final InstantCommand com_unlockClimber = new InstantCommand(sub_climber::unlockClimber);
private final InstantCommand com_pivotClimberPerpendicular = new InstantCommand(sub_climber::pivotPerpendicular);
Expand Down Expand Up @@ -198,7 +215,8 @@ private void configureButtonBindings() {
coDriverStick.POV_North.whenPressed(com_presetFender);
coDriverStick.POV_East.whenPressed(com_presetTarmacUpper);
coDriverStick.POV_South.whenPressed(com_presetLaunchpadUpper);
coDriverStick.POV_West.whenPressed(com_presetTerminalUpper);
coDriverStick.POV_South.whenPressed(new SetTurretPosition(sub_turret, TurretPrefs.turretMaxAngleDegrees));
coDriverStick.POV_West.whenPressed(com_presetTarmacUpper);

coDriverStick.btn_RStick.whenPressed(com_presetBackward);
coDriverStick.btn_LStick.whenPressed(com_presetForward);
Expand Down Expand Up @@ -276,6 +294,7 @@ private void configureDashboardButtons() {

public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return new OpenLoopTwoBall(sub_drivetrain, sub_shooter, sub_turret, sub_hood, sub_transfer, sub_intake);
return new OpenLoopTwoBall(sub_drivetrain, sub_shooter, sub_turret, sub_hood, sub_transfer, sub_intake,
sub_climber);
}
}
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/RobotPreferences.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ public static final class ShooterPrefs {
public static final SN_DoublePreference shooterTargetRPM = new SN_DoublePreference(
"shooterTargetRPM", 5000);
public static final SN_DoublePreference shooterAcceptableErrorRPM = new SN_DoublePreference(
"shooterAcceptableErrorRPM", 100);
"shooterAcceptableErrorRPM", 15);

public static final SN_BooleanPreference shooterInvert = new SN_BooleanPreference(
"shooterInvert", false);
Expand All @@ -118,7 +118,7 @@ public static final class ShooterPrefs {
public static final SN_DoublePreference shooterPresetUpperTarmacRPM = new SN_DoublePreference(
"shooterPresetUpperTarmacRPM", 3400);
public static final SN_DoublePreference shooterPresetUpperLaunchpadRPM = new SN_DoublePreference(
"shooterPresetUpperLaunchpadRPM", 6000);
"shooterPresetUpperLaunchpadRPM", 4700);
public static final SN_DoublePreference shooterPresetUpperTerminalRPM = new SN_DoublePreference(
"shooterPresetUpperTerminalRPM", 5000);

Expand Down Expand Up @@ -281,7 +281,7 @@ public static final class FourCargoA {

// auto4
public static final class OpenLoopTwoBall {
public static final SN_DoublePreference auto4shooterRPM = new SN_DoublePreference("auto4shooterRPM", 3100);
public static final SN_DoublePreference auto4shooterRPM = new SN_DoublePreference("auto4shooterRPM", 3000);
public static final SN_BooleanPreference auto4hoodSteep = new SN_BooleanPreference("auto4hoodSteep", false);
public static final SN_DoublePreference auto4dist1 = new SN_DoublePreference("auto4dist1", 44444);
public static final SN_DoublePreference auto4dist2 = new SN_DoublePreference("auto4dist2", -82000);
Expand Down
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/commands/Autonomous/OpenLoopTwoBall.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import frc.robot.commands.Shooter.PresetShooter;
import frc.robot.commands.Shooter.SpinFlywheelGoalRPM;
import frc.robot.commands.Transfer.PushCargoSimple;
import frc.robot.subsystems.Climber;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hood;
import frc.robot.subsystems.Intake;
Expand All @@ -31,23 +32,26 @@ public class OpenLoopTwoBall extends SequentialCommandGroup {
Hood hood;
Transfer transfer;
Intake intake;
Climber climber;

/** Creates a new OpenLoopTwoBall. */
public OpenLoopTwoBall(Drivetrain sub_drivetrain, Shooter sub_shooter, Turret sub_turret, Hood sub_hood,
Transfer sub_transfer, Intake sub_intake) {
Transfer sub_transfer, Intake sub_intake, Climber sub_climber) {

drivetrain = sub_drivetrain;
shooter = sub_shooter;
turret = sub_turret;
hood = sub_hood;
transfer = sub_transfer;
intake = sub_intake;

climber = sub_climber;
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(
new InstantCommand(drivetrain::resetDrivetrainEncodersCount),
new InstantCommand(shooter::setGoalUpperHub),
new InstantCommand(drivetrain::setCoastMode),
new InstantCommand(climber::pivotAngled),
new PresetShooter(shooter, hood, AutoPrefs.OpenLoopTwoBall.auto4shooterRPM,
AutoPrefs.OpenLoopTwoBall.auto4hoodSteep, null, null),

Expand All @@ -58,6 +62,7 @@ public OpenLoopTwoBall(Drivetrain sub_drivetrain, Shooter sub_shooter, Turret su
new DriveDistanceOpenLoop(drivetrain, AutoPrefs.OpenLoopTwoBall.auto4dist2,
DrivetrainPrefs.driveOpenLoopSpeedReverse),
new WaitCommand(2),
new InstantCommand(drivetrain::configure),
parallel(new SpinFlywheelGoalRPM(shooter), new PushCargoSimple(shooter, transfer)).withTimeout(8)

);
Expand Down
15 changes: 10 additions & 5 deletions src/main/java/frc/robot/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,10 @@ public void setClimberSpeed(double a_speed) {
}

if (isClimberAtBottom() && speed < 0) {
speed = 0;
resetClimberEncoderCount();
if (speed < 0) {
speed = 0;
}
}

climbMotor.set(ControlMode.PercentOutput, speed);
Expand Down Expand Up @@ -149,13 +151,16 @@ public boolean isClimberClosedLoopErrorAcceptable() {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Climber Encoder Counts", getClimberEncoderCount());
SmartDashboard.putNumber("Climber Closed Loop Error", getClimberClosedLoopError());
// SmartDashboard.putNumber("Climber Encoder Counts", getClimberEncoderCount());
// SmartDashboard.putNumber("Climber Closed Loop Error",
// getClimberClosedLoopError());
SmartDashboard.putBoolean("Is Climber At Bottom", isClimberAtBottom());
SmartDashboard.putBoolean("Is Climber Locked", isClimberLocked());
SmartDashboard.putBoolean("Is Climber Angled", isClimberAngled());
SmartDashboard.putBoolean("Is Climber Hooked", isHookDeployed());
SmartDashboard.putBoolean("Is Climber Error Acceptable", isClimberClosedLoopErrorAcceptable());
SmartDashboard.putNumber("Climber Motor Speed", climbMotor.getMotorOutputPercent());
// SmartDashboard.putBoolean("Is Climber Error Acceptable",
// isClimberClosedLoopErrorAcceptable());
// SmartDashboard.putNumber("Climber Motor Speed",
// climbMotor.getMotorOutputPercent());
}
}
97 changes: 64 additions & 33 deletions src/main/java/frc/robot/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,42 +219,73 @@ public double getAverageClosedLoopErrorInches() {
return (getLeftClosedLoopErrorInches() + getRightClosedLoopErrorInches()) / 2;
}

public void setBrakeMode() {
leftFollowMotor.setNeutralMode(NeutralMode.Brake);
leftLeadMotor.setNeutralMode(NeutralMode.Brake);

rightFollowMotor.setNeutralMode(NeutralMode.Brake);
rightLeadMotor.setNeutralMode(NeutralMode.Brake);
}

public void setCoastMode() {
leftFollowMotor.setNeutralMode(NeutralMode.Coast);
leftLeadMotor.setNeutralMode(NeutralMode.Coast);

rightFollowMotor.setNeutralMode(NeutralMode.Coast);
rightLeadMotor.setNeutralMode(NeutralMode.Coast);
}

@Override
public void periodic() {
// This method will be called once per scheduler run

// Encoder Counts
SmartDashboard.putNumber("Drivetrain Left Encoder", getLeftEncoderCount());
SmartDashboard.putNumber("Drivetrain Right Encoder", getRightEncoderCount());
SmartDashboard.putNumber("Drivetrain Average Encoder", getAverageEncoderCount());

// Motion Profile
SmartDashboard.putBoolean("Is Drivetrain Motion Profile Finished", isMotionProfileFinished());

// Motor Percent Output
SmartDashboard.putNumber("Drivetrain Left Lead Motor Speed", leftLeadMotor.getMotorOutputPercent());
SmartDashboard.putNumber("Drivetrain Right Lead Motor Speed", rightLeadMotor.getMotorOutputPercent());
SmartDashboard.putNumber("Drivetrain Left Follow Motor Speed", leftFollowMotor.getMotorOutputPercent());
SmartDashboard.putNumber("Drivetrain Right Follow Motor Speed", rightFollowMotor.getMotorOutputPercent());

// Feet Driven
SmartDashboard.putNumber("Drivetrain Left Feet Driven", getLeftFeetDriven());
SmartDashboard.putNumber("Drivetrain Right Feet Driven", getRightFeetDriven());
SmartDashboard.putNumber("Drivetrain Average Feet Driven", getAverageFeetDriven());

// Feet Per Second
SmartDashboard.putNumber("Drivetrain Left Feet Per Second", getLeftFeetPerSecond());
SmartDashboard.putNumber("Drivetrain Right Feet Per Second", getRightFeetPerSecond());
SmartDashboard.putNumber("Drivetrain Average Feet Per Second", getAverageFeetPerSecond());

// Encoder Counts per 100ms
SmartDashboard.putNumber("Drivetrain Left Velocity", getLeftVelocity());
SmartDashboard.putNumber("Drivetrain Right Velocity", getRightVelocity());
SmartDashboard.putNumber("Drivetrain Average Velocity", getAverageVelocity());

// Closed Loop Error
SmartDashboard.putNumber("Drivetrain Left Closed Loop Error Inches", getLeftClosedLoopErrorInches());
SmartDashboard.putNumber("Drivetrain Right Closed Loop Error Inches", getRightClosedLoopErrorInches());
SmartDashboard.putNumber("Drivetrain Average Closed Loop Error Inches", getAverageClosedLoopErrorInches());
// // Encoder Counts
// SmartDashboard.putNumber("Drivetrain Left Encoder", getLeftEncoderCount());
// SmartDashboard.putNumber("Drivetrain Right Encoder", getRightEncoderCount());
// SmartDashboard.putNumber("Drivetrain Average Encoder",
// getAverageEncoderCount());

// // Motion Profile
// SmartDashboard.putBoolean("Is Drivetrain Motion Profile Finished",
// isMotionProfileFinished());

// // Motor Percent Output
// SmartDashboard.putNumber("Drivetrain Left Lead Motor Speed",
// leftLeadMotor.getMotorOutputPercent());
// SmartDashboard.putNumber("Drivetrain Right Lead Motor Speed",
// rightLeadMotor.getMotorOutputPercent());
// SmartDashboard.putNumber("Drivetrain Left Follow Motor Speed",
// leftFollowMotor.getMotorOutputPercent());
// SmartDashboard.putNumber("Drivetrain Right Follow Motor Speed",
// rightFollowMotor.getMotorOutputPercent());

// // Feet Driven
// SmartDashboard.putNumber("Drivetrain Left Feet Driven", getLeftFeetDriven());
// SmartDashboard.putNumber("Drivetrain Right Feet Driven",
// getRightFeetDriven());
// SmartDashboard.putNumber("Drivetrain Average Feet Driven",
// getAverageFeetDriven());

// // Feet Per Second
// SmartDashboard.putNumber("Drivetrain Left Feet Per Second",
// getLeftFeetPerSecond());
// SmartDashboard.putNumber("Drivetrain Right Feet Per Second",
// getRightFeetPerSecond());
// SmartDashboard.putNumber("Drivetrain Average Feet Per Second",
// getAverageFeetPerSecond());

// // Encoder Counts per 100ms
// SmartDashboard.putNumber("Drivetrain Left Velocity", getLeftVelocity());
// SmartDashboard.putNumber("Drivetrain Right Velocity", getRightVelocity());
// SmartDashboard.putNumber("Drivetrain Average Velocity",
// getAverageVelocity());

// // Closed Loop Error
// SmartDashboard.putNumber("Drivetrain Left Closed Loop Error Inches",
// getLeftClosedLoopErrorInches());
// SmartDashboard.putNumber("Drivetrain Right Closed Loop Error Inches",
// getRightClosedLoopErrorInches());
// SmartDashboard.putNumber("Drivetrain Average Closed Loop Error Inches",
// getAverageClosedLoopErrorInches());
}
}
Loading

0 comments on commit 62c74cb

Please sign in to comment.