diff --git a/.githooks/pre-commit b/.githooks/pre-commit index 5411828..de23d8b 100755 --- a/.githooks/pre-commit +++ b/.githooks/pre-commit @@ -1,5 +1,8 @@ #!/bin/sh +exit 0 exec 1>&2 chmod +x gradlew echo "Formatting code before commit..." -exec ./gradlew -q syncGitHooks format +./gradlew -q syncGitHooks format +[[ $? != 0 ]] && exit 1 # exit if formatter failed +exec git add -A ./src/main/java/frc/robot diff --git a/.gitignore b/.gitignore index 986fc27..0a45946 100644 --- a/.gitignore +++ b/.gitignore @@ -116,3 +116,4 @@ gradle-app.setting .settings/ bin/ imgui.ini +update_with_main.zsh diff --git a/.vscode/settings.json b/.vscode/settings.json index e31073b..d89f96c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -24,6 +24,8 @@ "**/.project": true, "**/.settings": true, "**/.factorypath": true, - "**/*~": true + "**/*~": true, + + "WPILib-License.md": true } } diff --git a/build.gradle b/build.gradle index 219dace..604b349 100644 --- a/build.gradle +++ b/build.gradle @@ -56,7 +56,7 @@ dependencies { simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false) - implementation files('libs/botplus-v0.5.2.jar') + implementation files('libs/botplus-v0.6.3.jar') // Websocket extensions require additional configuration. // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false) diff --git a/docs/Controls.pdf b/docs/Controls.pdf new file mode 100644 index 0000000..dec6d7f Binary files /dev/null and b/docs/Controls.pdf differ diff --git a/docs/Controls.pub b/docs/Controls.pub new file mode 100644 index 0000000..e537bed Binary files /dev/null and b/docs/Controls.pub differ diff --git a/libs/botplus-v0.6.3-sources.jar b/libs/botplus-v0.6.3-sources.jar new file mode 100644 index 0000000..e14326c Binary files /dev/null and b/libs/botplus-v0.6.3-sources.jar differ diff --git a/libs/botplus-v0.5.2.jar b/libs/botplus-v0.6.3.jar similarity index 94% rename from libs/botplus-v0.5.2.jar rename to libs/botplus-v0.6.3.jar index e45d938..de4edc4 100644 Binary files a/libs/botplus-v0.5.2.jar and b/libs/botplus-v0.6.3.jar differ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b90ee8c..6cbe495 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,49 +1,105 @@ package frc.robot; -import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.auton.sequences.*; +import frc.robot.commands.teleop.*; +import frc.robot.commands.teleop.automatic.*; +import frc.robot.subsystems.*; +import org.awtybots.frc.botplus.CompetitionBot; +import org.awtybots.frc.botplus.commands.Controller; +import org.awtybots.frc.botplus.sensors.vision.Limelight; -public class Robot extends TimedRobot { +public class Robot extends CompetitionBot { + + public static Limelight limelight = + new Limelight( + RobotMap.Dimensions.limelightMountingHeight, RobotMap.Dimensions.limelightMountingAngle); + public static PowerDistributionPanel pdp = new PowerDistributionPanel(); + + private Compressor compressor = new Compressor(); @Override - public void robotInit() {} + public void robotInit() { + super.robotInit(); + compressor.setClosedLoopControl(true); + } - /** - * This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard - * integrated updating. - */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. CommandScheduler.getInstance().run(); } @Override - public void disabledInit() {} + public boolean isTestMode() { + return false; + } @Override - public void disabledPeriodic() {} + public void addAutonOptions() { + addAutonDefault("Drive Forward Time and Shoot 3", new DriveForwardTimeAndShoot3(0.75)); + // addAutonOption("Shoot 3 and Drive Forward Time", new Shoot3AndDriveForwardTime(1.5)); + // addAutonOption("Shoot 3 from the Line", new Shoot3FromLine()); + // addAutonOption("Drive Forward Time", new DriveForwardTime(1.5)); + } @Override - public void autonomousInit() {} + public void teleopInit() { + super.teleopInit(); + Robot.limelight.setDriverMode(true); + } @Override - public void autonomousPeriodic() {} + public void teleopPeriodic() { + super.teleopPeriodic(); + } @Override - public void teleopInit() {} + public void disabledInit() { + super.disabledInit(); + Robot.limelight.setDriverMode(true); + } @Override - public void teleopPeriodic() {} + public void bindIO() { + Controller controller1 = new Controller(0); + Controller controller2 = new Controller(1); - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } + controller1.streamAnalogInputTo(new TeleopDrive()); + controller1.getTrgL().whenHeld(new ToggleIntakeMotorOnly()); + controller1.getTrgR().whenHeld(new ToggleIntake()); + controller1.getBtnA().whenHeld(new ToggleClimber(ToggleClimber.Forward)); + controller1.getBtnBack().whenHeld(new ToggleClimber(ToggleClimber.Reverse)); - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} + // controller1.getBtnX().whenHeld(new ManualHoodRun(true)); + // controller1.getBtnB().whenHeld(new ManualHoodRun(false)); + + controller2.getBtnA().whenHeld(new ManualShootingPreset(4000.0, 76, false)); // against wall + controller2.getBtnX().whenHeld(new ManualShootingPreset(5000.0, 58, true)); // mid range? + controller2.getBtnB().whenHeld(new ManualShootingPreset(5700.0, 57, true)); // long range + // controller2 + // .getBtnY() + // .whenHeld( + // new AutoAimUsingTurret() + // .alongWith(new AutoShoot())); // fancy pants shot w/limelight, turret, physics + // math + + controller2.getBtnBack().whenPressed(new ResetHoodDumb()); + // new ResetTurret() // hood all the way back, turret dead center + // .alongWith(new SetHoodLaunchAngle(AdjustableHoodSubsystem.maxLaunchAngle))); + // controller2.getBtnStart().whenHeld(new AutoAimUsingTurret()); + // controller2.getDpadLeft().whenPressed( // manually move turret left 10 degrees + // new InstantCommand(() -> TurretSubsystem.getInstance().setRelativeGoalAngle(-10), + // TurretSubsystem.getInstance() + // )); + // controller2.getDpadRight().whenPressed( // manually move turret right 10 degrees + // new InstantCommand(() -> TurretSubsystem.getInstance().setRelativeGoalAngle(10), + // TurretSubsystem.getInstance() + // )); + controller2.getBmpL().whenHeld(new ReverseTower()); + controller2.getBmpR().whenHeld(new ToggleTower()); + controller2.getTrgL().whenHeld(new ReverseIndexer()); + controller2.getTrgR().whenHeld(new ToggleIndexer()); + } } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java new file mode 100644 index 0000000..7a6be86 --- /dev/null +++ b/src/main/java/frc/robot/RobotMap.java @@ -0,0 +1,72 @@ +package frc.robot; + +public abstract class RobotMap { + public abstract static class CAN { + public static final int leftDrive1 = 13; + public static final int leftDrive2 = 14; + public static final int rightDrive1 = 11; + public static final int rightDrive2 = 12; + + public static final int intake = 5; + // public static final int spindexer = 7; // rip + public static final int tower = 9; + + public static final int turret = 8; + public static final int adjustableHood = 10; + public static final int shooter = 15; + + public static final int indexerL = 7; + public static final int indexerR = 6; + + public static final int climber = 16; + } + + public abstract static class PDP { + public static final int spindexer = 4; + public static final int tower = 13; + public static final int adjustableHood = 12; + public static final int climber = 2; + } + + public abstract static class PCM { + public static final int intakeFwd = 5; + public static final int intakeRev = 4; + } + + public abstract static class DIO { + public static final int allianceColorLEDs = 0; + public static final int towerLimitSwitch = 1; + } + + public abstract static class LimelightPipelines { + public static final int powerPort = 0; + } + + public abstract static class Dimensions { + public static final double limelightMountingAngle = 23; // degrees TODO doublecheck + public static final double limelightMountingHeight = feetInchesMeters(20.9); // TODO doublecheck + + public static final double powerPortHeight = feetInchesMeters(8, 2.25); + public static final double powerPortVisionTargetOffset = + feetInchesMeters(8.5); // distance between center of vision target and center of goal + + public static final double trackWidth = + feetInchesMeters(26.755); // distance between the left and right wheels, meters + + public static final double climberWinchCircumference = + 3; // this one is actually in inches believe it or not + } + + public static double feetInchesMeters(int feet, double inches) { + inches += feet * 12.0; + double centimeters = inches * 2.54; + double meters = centimeters / 100.0; + return meters; + } + + public static double feetInchesMeters(double inches) { + double centimeters = inches * 2.54; + double meters = centimeters / 100.0; + return meters; + } +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index ea06384..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import static frc.robot.Robot.*; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ExampleCommand extends CommandBase { - private boolean button; - private boolean on; - - public ExampleCommand() {} - - @Override - public void initialize() {} - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - return false; // return true when you want the command to stop running - } -} diff --git a/src/main/java/frc/robot/commands/auton/DriveCurve.java b/src/main/java/frc/robot/commands/auton/DriveCurve.java new file mode 100644 index 0000000..3ab1c2e --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/DriveCurve.java @@ -0,0 +1,25 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotMap; + +public class DriveCurve extends SequentialCommandGroup { + + /** + * a smooth arc path for the robot to follow + * + * @param arcRadius radius of arc drawn by path of robot + * @param degrees degrees of circle to make arc from (positive means curve right) + */ + public DriveCurve(double arcRadius, double degrees) { + double radians = Math.toRadians(degrees); + + double centerArcLength = arcRadius * radians; + double differentialArcLength = (RobotMap.Dimensions.trackWidth * 0.5) * radians; + + double leftArcLength = centerArcLength + differentialArcLength; + double rightArcLength = centerArcLength - differentialArcLength; + + addCommands(new DriveDistance(leftArcLength, rightArcLength)); + } +} diff --git a/src/main/java/frc/robot/commands/auton/DriveDistance.java b/src/main/java/frc/robot/commands/auton/DriveDistance.java new file mode 100644 index 0000000..b4e1a48 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/DriveDistance.java @@ -0,0 +1,58 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class DriveDistance extends CommandBase { + private static final double gapTime = 0.5; + + private DriveSideDistance driveSideDistanceL; + private DriveSideDistance driveSideDistanceR; + + private double plannedTime; + private Timer timer; + + public DriveDistance(double distance) { + this(distance, distance); + } + + public DriveDistance(double distanceL, double distanceR) { + addRequirements(DrivetrainSubsystem.getInstance()); + driveSideDistanceL = new DriveSideDistance(distanceL); + driveSideDistanceR = new DriveSideDistance(distanceR); + + if (driveSideDistanceL.plannedTime < driveSideDistanceR.plannedTime) { + plannedTime = driveSideDistanceR.plannedTime; + driveSideDistanceL.replanSlower(plannedTime); + } else { + plannedTime = driveSideDistanceL.plannedTime; + driveSideDistanceR.replanSlower(plannedTime); + } + + timer = new Timer(); + } + + @Override + public void initialize() { + timer.start(); + } + + public void execute() { + double currentTime = timer.get(); + double velocityL = driveSideDistanceL.tick(currentTime); + double velocityR = driveSideDistanceR.tick(currentTime); + DrivetrainSubsystem.getInstance().setMotorVelocityOutput(velocityL, velocityR); + } + + @Override + public void end(boolean interrupted) { + if (interrupted) DrivetrainSubsystem.getInstance().kill(); + else DrivetrainSubsystem.getInstance().softStop(); + } + + @Override + public boolean isFinished() { + return timer.get() > plannedTime + gapTime; + } +} diff --git a/src/main/java/frc/robot/commands/auton/DriveSideDistance.java b/src/main/java/frc/robot/commands/auton/DriveSideDistance.java new file mode 100644 index 0000000..37dd32b --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/DriveSideDistance.java @@ -0,0 +1,72 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.wpiutil.math.MathUtil; +import frc.robot.subsystems.DrivetrainSubsystem; + +class DriveSideDistance { + private static final double defaultMaxVelocity = 0.5; // meters per second + private static final double defaultMaxAcceleration = 0.25; // meters per second per second + + private double maxVelocity; + private double maxAcceleration; + + private double distanceGoal; + double plannedTime; + + private double decelerationStartTime; + + private double outputFactor = 1.0; + + DriveSideDistance(double distance) { + distanceGoal = distance; + + if (distanceGoal < 0.0) { + distanceGoal = -distanceGoal; + outputFactor = -1.0; + } + + planFastest(); + } + + private void planFastest() { + maxVelocity = defaultMaxVelocity; + maxAcceleration = defaultMaxAcceleration; + + double timeToAccelerate = maxVelocity / maxAcceleration; + double distanceWhileAccelerating = + maxVelocity * timeToAccelerate; // accounts for both accel at beginning and decel + // at the end + + if (distanceGoal >= distanceWhileAccelerating) { + plannedTime = + timeToAccelerate * 2.0 + (distanceGoal - distanceWhileAccelerating) / maxVelocity; + decelerationStartTime = plannedTime - timeToAccelerate; + } else { + plannedTime = 2.0 * Math.sqrt(distanceGoal / maxAcceleration); + decelerationStartTime = plannedTime / 2.0; + maxVelocity = maxAcceleration * decelerationStartTime; + } + } + + void replanSlower(double newTime) { + double factor = plannedTime / newTime; + maxVelocity *= factor; + maxAcceleration *= factor; + } + + double tick(double currentTime) { + double newVelocity; + + if (currentTime > plannedTime) { + newVelocity = 0.0; + } else if (currentTime > decelerationStartTime) { + newVelocity = maxVelocity - maxAcceleration * (currentTime - decelerationStartTime); + } else { + newVelocity = Math.min(maxAcceleration * currentTime, maxVelocity); + } + + return MathUtil.clamp(newVelocity, 0.0, maxVelocity) + * outputFactor + / DrivetrainSubsystem.getInstance().getConfig().getVelocityPeak(); + } +} diff --git a/src/main/java/frc/robot/commands/auton/RotateAngle.java b/src/main/java/frc/robot/commands/auton/RotateAngle.java new file mode 100644 index 0000000..eb7ef76 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/RotateAngle.java @@ -0,0 +1,7 @@ +package frc.robot.commands.auton; + +public class RotateAngle extends DriveCurve { + public RotateAngle(double degrees) { + super(0.0, degrees); + } +} diff --git a/src/main/java/frc/robot/commands/auton/sequences/DriveBackwardsTime.java b/src/main/java/frc/robot/commands/auton/sequences/DriveBackwardsTime.java new file mode 100644 index 0000000..948c589 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/sequences/DriveBackwardsTime.java @@ -0,0 +1,16 @@ +package frc.robot.commands.auton.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class DriveBackwardsTime extends SequentialCommandGroup { + public DriveBackwardsTime(double time) { + addCommands( + new StartEndCommand( + () -> DrivetrainSubsystem.getInstance().setMotorRawOutput(-0.3, -0.3), + () -> DrivetrainSubsystem.getInstance().kill(), + DrivetrainSubsystem.getInstance()) + .withTimeout(time)); + } +} diff --git a/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTime.java b/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTime.java new file mode 100644 index 0000000..607e268 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTime.java @@ -0,0 +1,16 @@ +package frc.robot.commands.auton.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class DriveForwardTime extends SequentialCommandGroup { + public DriveForwardTime(double time) { + addCommands( + new StartEndCommand( + () -> DrivetrainSubsystem.getInstance().setMotorRawOutput(0.3, 0.3), + () -> DrivetrainSubsystem.getInstance().kill(), + DrivetrainSubsystem.getInstance()) + .withTimeout(time)); + } +} diff --git a/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTimeAndShoot3.java b/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTimeAndShoot3.java new file mode 100644 index 0000000..9021c0d --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/sequences/DriveForwardTimeAndShoot3.java @@ -0,0 +1,10 @@ +package frc.robot.commands.auton.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.teleop.automatic.ManualShootingPreset; + +public class DriveForwardTimeAndShoot3 extends SequentialCommandGroup { + public DriveForwardTimeAndShoot3(double time) { + addCommands(new DriveForwardTime(time), new ManualShootingPreset(3700, 76, false)); + } +} diff --git a/src/main/java/frc/robot/commands/auton/sequences/Shoot3AndDriveForwardTime.java b/src/main/java/frc/robot/commands/auton/sequences/Shoot3AndDriveForwardTime.java new file mode 100644 index 0000000..a981042 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/sequences/Shoot3AndDriveForwardTime.java @@ -0,0 +1,11 @@ +package frc.robot.commands.auton.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.teleop.automatic.ManualShootingPreset; + +public class Shoot3AndDriveForwardTime extends SequentialCommandGroup { + public Shoot3AndDriveForwardTime(double time) { + addCommands( + new ManualShootingPreset(4800, 56, false).withTimeout(10), new DriveForwardTime(time)); + } +} diff --git a/src/main/java/frc/robot/commands/auton/sequences/Shoot3FromLine.java b/src/main/java/frc/robot/commands/auton/sequences/Shoot3FromLine.java new file mode 100644 index 0000000..4e25c48 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/sequences/Shoot3FromLine.java @@ -0,0 +1,10 @@ +package frc.robot.commands.auton.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.teleop.automatic.ManualShootingPreset; + +public class Shoot3FromLine extends SequentialCommandGroup { + public Shoot3FromLine() { + addCommands(new ManualShootingPreset(4800, 56, false).withTimeout(10)); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ReverseIndexer.java b/src/main/java/frc/robot/commands/teleop/ReverseIndexer.java new file mode 100644 index 0000000..897b207 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ReverseIndexer.java @@ -0,0 +1,25 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IndexerSubsystem; + +public class ReverseIndexer extends CommandBase { + public ReverseIndexer() { + addRequirements(IndexerSubsystem.getInstance()); + } + + @Override + public void initialize() { + IndexerSubsystem.getInstance().set(-1); + } + + @Override + public void end(boolean interrupted) { + IndexerSubsystem.getInstance().set(0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ReverseIntake.java b/src/main/java/frc/robot/commands/teleop/ReverseIntake.java new file mode 100644 index 0000000..0e3244b --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ReverseIntake.java @@ -0,0 +1,25 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IntakeSubsystem; + +public class ReverseIntake extends CommandBase { + public ReverseIntake() { + addRequirements(IntakeSubsystem.getInstance()); + } + + @Override + public void initialize() { + IntakeSubsystem.getInstance().reverse(); + } + + @Override + public void end(boolean interrupted) { + IntakeSubsystem.getInstance().toggle(false); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ReverseTower.java b/src/main/java/frc/robot/commands/teleop/ReverseTower.java new file mode 100644 index 0000000..74735c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ReverseTower.java @@ -0,0 +1,25 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.TowerSubsystem; + +public class ReverseTower extends CommandBase { + public ReverseTower() { + addRequirements(TowerSubsystem.getInstance()); + } + + @Override + public void initialize() { + TowerSubsystem.getInstance().reverse(); + } + + @Override + public void end(boolean interrupted) { + TowerSubsystem.getInstance().stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/teleop/TeleopDrive.java b/src/main/java/frc/robot/commands/teleop/TeleopDrive.java new file mode 100644 index 0000000..9ae5de6 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/TeleopDrive.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.DrivetrainSubsystem; +import org.awtybots.frc.botplus.commands.AnalogInputCommand; +import org.awtybots.frc.botplus.commands.ControllerValues; +import org.awtybots.frc.botplus.math.Vector2; + +public class TeleopDrive extends AnalogInputCommand { + + public TeleopDrive() { + addRequirements(DrivetrainSubsystem.getInstance()); + } + + @Override + public void analogExecute(ControllerValues controllerValues) { + Vector2 driveControlsInput = splitArcadeDrive(controllerValues); + + SmartDashboard.putNumber("Drive Controls L", driveControlsInput.getX()); + SmartDashboard.putNumber("Drive Controls R", driveControlsInput.getY()); + DrivetrainSubsystem.getInstance() + .setMotorRawOutput( // uncomment this for no PID drive + // .setMotorVelocityOutput( // comment this for no PID drive + driveControlsInput.getX(), driveControlsInput.getY()); + } + + private double smoothingFunction(double x) { + // return x; // simple passthrough + return (x * 0.3) + (Math.pow(x, 7) * 0.7); // joe drive + } + + private Vector2 splitArcadeDrive(ControllerValues controllerValues) { + double speed = smoothingFunction(controllerValues.getLeftStickY()); + double steer = smoothingFunction(controllerValues.getRightStickX()); + + if (speed < 0) { + steer *= -1; + } + + double left = speed + steer; + double right = speed - steer; + + return new Vector2(left, right); + } + + @SuppressWarnings("unused") + private Vector2 gtaDrive(ControllerValues controllerValues) { + double speed = + smoothingFunction(controllerValues.getRightTrigger() - controllerValues.getLeftTrigger()); + double steer = smoothingFunction(controllerValues.getLeftStickX()); + + if (speed < 0) { + steer *= -1; + } + + double left = speed + steer; + double right = speed - steer; + + return new Vector2(left, right); + } + + @Override + public void end(boolean interrupted) { + DrivetrainSubsystem.getInstance().kill(); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleClimber.java b/src/main/java/frc/robot/commands/teleop/ToggleClimber.java new file mode 100644 index 0000000..21af576 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleClimber.java @@ -0,0 +1,27 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ClimbSubsystem; + +public class ToggleClimber extends CommandBase { + + public static final int Forward = 1; + public static final int Reverse = -1; + + private final int dir; + + public ToggleClimber(int dir) { + this.dir = dir; + addRequirements(ClimbSubsystem.getInstance()); + } + + @Override + public void initialize() { + ClimbSubsystem.getInstance().set(dir); + } + + @Override + public void end(boolean interrupted) { + ClimbSubsystem.getInstance().set(0); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleIndexer.java b/src/main/java/frc/robot/commands/teleop/ToggleIndexer.java new file mode 100644 index 0000000..2a2610a --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleIndexer.java @@ -0,0 +1,22 @@ +package frc.robot.commands.teleop; + +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.TowerSubsystem; +import org.awtybots.frc.botplus.ToggleCommand; + +public class ToggleIndexer extends ToggleCommand { + public ToggleIndexer() { + super(IndexerSubsystem.getInstance(), TowerSubsystem.getInstance()); + } + + public ToggleIndexer(boolean on) { + super(on, IndexerSubsystem.getInstance(), TowerSubsystem.getInstance()); + } + + @Override + public void toggle(boolean b) { + IndexerSubsystem.getInstance().set(b ? 1 : 0); + if (b) TowerSubsystem.getInstance().enableForLoading(); + else TowerSubsystem.getInstance().stop(); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleIntake.java b/src/main/java/frc/robot/commands/teleop/ToggleIntake.java new file mode 100644 index 0000000..51ae846 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleIntake.java @@ -0,0 +1,20 @@ +package frc.robot.commands.teleop; + +import frc.robot.subsystems.IntakeSubsystem; +import org.awtybots.frc.botplus.ToggleCommand; + +public class ToggleIntake extends ToggleCommand { + + public ToggleIntake() { + super(IntakeSubsystem.getInstance()); + } + + public ToggleIntake(boolean on) { + super(on, IntakeSubsystem.getInstance()); + } + + @Override + public void toggle(boolean b) { + IntakeSubsystem.getInstance().toggle(b); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleIntakeMotorOnly.java b/src/main/java/frc/robot/commands/teleop/ToggleIntakeMotorOnly.java new file mode 100644 index 0000000..8379c89 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleIntakeMotorOnly.java @@ -0,0 +1,39 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IntakeSubsystem; + +public class ToggleIntakeMotorOnly extends CommandBase { + private boolean button; + private boolean on; + + public ToggleIntakeMotorOnly() { + button = true; + } + + public ToggleIntakeMotorOnly(boolean on) { + button = false; + this.on = on; + } + + @Override + public void execute() { + if (button) { + IntakeSubsystem.getInstance().toggleMotorOnly(true); + } else { + IntakeSubsystem.getInstance().toggleMotorOnly(on); + } + } + + @Override + public void end(boolean interrupted) { + if (button) { + IntakeSubsystem.getInstance().toggleMotorOnly(false); + } + } + + @Override + public boolean isFinished() { + return !button; + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleShooter.java b/src/main/java/frc/robot/commands/teleop/ToggleShooter.java new file mode 100644 index 0000000..674c178 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleShooter.java @@ -0,0 +1,47 @@ +package frc.robot.commands.teleop; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.TowerSubsystem; + +public class ToggleShooter extends CommandBase { + + private double rps; + private boolean startedTower = false; + private Timer timer = new Timer(); + + public ToggleShooter(double rpm) { + addRequirements( + ShooterSubsystem.getInstance(), + TowerSubsystem.getInstance(), + IndexerSubsystem.getInstance()); + rps = rpm / 60.0; + } + + @Override + public void initialize() { + ShooterSubsystem.getInstance().setFlywheelRevsPerSecond(rps); + startedTower = false; + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + if (timer.get() > 0.5 && !startedTower && ShooterSubsystem.getInstance().isFlywheelReady()) { + TowerSubsystem.getInstance().enableForShooting(); + IndexerSubsystem.getInstance().set(1); + startedTower = true; + } + } + + @Override + public void end(boolean interrupted) { + TowerSubsystem.getInstance().stop(); + IndexerSubsystem.getInstance().set(0); + ShooterSubsystem.getInstance().stopFlywheel(); + timer.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/ToggleTower.java b/src/main/java/frc/robot/commands/teleop/ToggleTower.java new file mode 100644 index 0000000..45c53b4 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/ToggleTower.java @@ -0,0 +1,21 @@ +package frc.robot.commands.teleop; + +import frc.robot.subsystems.TowerSubsystem; +import org.awtybots.frc.botplus.ToggleCommand; + +public class ToggleTower extends ToggleCommand { + + public ToggleTower() { + super(TowerSubsystem.getInstance()); + } + + public ToggleTower(boolean on) { + super(on, TowerSubsystem.getInstance()); + } + + @Override + public void toggle(boolean b) { + if (b) TowerSubsystem.getInstance().enableForShooting(); + else TowerSubsystem.getInstance().stop(); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingDrive.java b/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingDrive.java new file mode 100644 index 0000000..698f8af --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingDrive.java @@ -0,0 +1,64 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.RobotMap.LimelightPipelines; +import frc.robot.commands.auton.RotateAngle; + +public class AutoAimUsingDrive extends CommandBase { + + private static final double startupTime = 0.5; + + private RotateAngle rotateCommand = null; + private Timer timer; + + public AutoAimUsingDrive() {} // requirements are handled by RotateAngle command + + @Override + public void initialize() { + Robot.limelight.setPipeline(LimelightPipelines.powerPort); + + rotateCommand = null; + + timer = new Timer(); + timer.start(); + } + + @Override + public void execute() { + if (rotateCommand != null) { + if (rotateCommand.isFinished()) + rotateCommand = null; + else + return; + } + + if (timer.get() > startupTime) { + timer = null; + + boolean hasVisibleTarget = Robot.limelight.getHasVisibleTarget(); + SmartDashboard.putBoolean("Limelight Target Visible", hasVisibleTarget); + + if (hasVisibleTarget) { + double x = Robot.limelight.getXOffset(); + SmartDashboard.putNumber("Limelight Target X Offset", x); + + rotateCommand = new RotateAngle(x); + rotateCommand.schedule(); + } + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + if (interrupted) rotateCommand.cancel(); + Robot.limelight.setDriverMode(true); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingTurret.java b/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingTurret.java new file mode 100644 index 0000000..fd0dc1d --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/AutoAimUsingTurret.java @@ -0,0 +1,67 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.RobotMap.LimelightPipelines; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoAimUsingTurret extends CommandBase { + + private boolean hunting = false; + private boolean huntingToMax = true; + + public AutoAimUsingTurret() { + addRequirements(TurretSubsystem.getInstance()); + } + + @Override + public void initialize() { + Robot.limelight.setPipeline(LimelightPipelines.powerPort); + + hunting = false; + } + + @Override + public void execute() { + boolean hasVisibleTarget = Robot.limelight.getHasVisibleTarget(); + SmartDashboard.putBoolean("Limelight Target Visible", hasVisibleTarget); + + if (hasVisibleTarget) { + if (hunting) { + hunting = false; + } + } else { + if (hunting) { + if (TurretSubsystem.getInstance().atGoalAngle()) { + huntingToMax = !huntingToMax; + TurretSubsystem.getInstance() + .setAbsoluteGoalAngle( + huntingToMax ? TurretSubsystem.maxAngle : TurretSubsystem.minAngle); + } + } else { + hunting = true; + huntingToMax = true; + + TurretSubsystem.getInstance().setAbsoluteGoalAngle(TurretSubsystem.maxAngle); + } + return; + } + + double x = Robot.limelight.getXOffset(); + SmartDashboard.putNumber("Limelight Target X Offset", x); + + TurretSubsystem.getInstance().setRelativeGoalAngle(x); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + Robot.limelight.setDriverMode(true); + TurretSubsystem.getInstance().returnToStart(); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/AutoShoot.java b/src/main/java/frc/robot/commands/teleop/automatic/AutoShoot.java new file mode 100644 index 0000000..23443c2 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/AutoShoot.java @@ -0,0 +1,111 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.RobotMap.LimelightPipelines; +import frc.robot.subsystems.AdjustableHoodSubsystem; +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.TowerSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import org.awtybots.frc.botplus.Logger; +import org.awtybots.frc.botplus.math.Simulation; +import org.awtybots.frc.botplus.math.Vector2; +import org.awtybots.frc.botplus.math.VisionTarget; + +public class AutoShoot extends CommandBase { + + private Logger logger = new Logger("AutoShoot"); + + private VisionTarget powerPort; + private Simulation projectileMotionSimulation; + + public AutoShoot() { + addRequirements( + ShooterSubsystem.getInstance(), + TowerSubsystem.getInstance(), + IndexerSubsystem.getInstance(), + TurretSubsystem.getInstance(), + AdjustableHoodSubsystem.getInstance()); + + SmartDashboard.putBoolean("Projectile Motion Solution", true); + + powerPort = + new VisionTarget( + Robot.limelight, + 2.496 - 0.216, // vision target height + 0.216 // offset from vision target to center of power port + ); + projectileMotionSimulation = + new Simulation( + 0.01, // simulation step (seconds) + 10, // simulation iterations + 0.178, // ball radius (m) + 0.142, // ball mass (kg) + 45, // launch angle (degrees), gets overwritten periodically so this doesn't matter + ShooterSubsystem.getInstance().flywheel.getMaxBallVelocity(), + false // debug mode + ); + } + + @Override + public void initialize() { + Robot.limelight.setPipeline(LimelightPipelines.powerPort); + } + + @Override + public void execute() { + if (!Robot.limelight.getHasVisibleTarget()) { + logger.warn("Cannot auto-shoot without visible target!"); + // return; + } + + // TODO uncomment this once everything else is working + Vector2 powerPortOffset = powerPort.getTargetDisplacement(); + SmartDashboard.putNumber("Power Port Perceived Distance", powerPortOffset.getX()); + + double adjustableHoodGoalLaunchAngle = + Math.toDegrees(Math.atan2(powerPortOffset.getX(), powerPortOffset.getY() + 1.0)); + + // AdjustableHoodSubsystem.getInstance().setGoalLaunchAngle(adjustableHoodGoalLaunchAngle); // + // TODO fix + + projectileMotionSimulation.setLaunchAngle(45); // TODO fix + // AdjustableHoodSubsystem.getInstance().getCurrentLaunchAngle()); + Vector2 velocity = projectileMotionSimulation.findOptimalLaunchVelocity(powerPortOffset); + // Vector2 velocity = null; // TODO remove and uncomment lines above + if (velocity == null) { + SmartDashboard.putBoolean("Projectile Motion Solution", false); + logger.warn( + "Projectile motion simulation found no solution! Move the robot to a better shooting position."); + return; + } + SmartDashboard.putBoolean("Projectile Motion Solution", true); + double goalRevsPerSecond = + ShooterSubsystem.getInstance().flywheel.ballVelocityToMotorRpm(velocity) / 60.0; + ShooterSubsystem.getInstance().setFlywheelRevsPerSecond(goalRevsPerSecond); + + boolean readyToShoot = + TurretSubsystem.getInstance().atGoalAngle() + // && AdjustableHoodSubsystem.getInstance().atGoalLaunchAngle() // TODO fix + && ShooterSubsystem.getInstance().isFlywheelReady(); + + if (readyToShoot) { + TowerSubsystem.getInstance().enableForShooting(); + IndexerSubsystem.getInstance().set(1); + } else { + TowerSubsystem.getInstance().stop(); + IndexerSubsystem.getInstance().set(0); + } + } + + @Override + public void end(boolean interrupted) { + ShooterSubsystem.getInstance().stopFlywheel(); + TowerSubsystem.getInstance().stop(); + IndexerSubsystem.getInstance().set(0); + + Robot.limelight.setDriverMode(true); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/ManualHoodRun.java b/src/main/java/frc/robot/commands/teleop/automatic/ManualHoodRun.java new file mode 100644 index 0000000..eaab691 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/ManualHoodRun.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.teleop.automatic; + +// import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.subsystems.AdjustableHoodSubsystem; + +public class ManualHoodRun extends SequentialCommandGroup { + public ManualHoodRun(boolean forwards) { // TODO fix + // double output = forwards ? 0.15 : -0.15; + addCommands( + new StartEndCommand( + () -> AdjustableHoodSubsystem.getInstance(), + // .motor + // .getMotorController() + // .set(ControlMode.PercentOutput, output), + () -> AdjustableHoodSubsystem.getInstance(), + // .motor + // .getMotorController() + // .set(ControlMode.PercentOutput, 0), + AdjustableHoodSubsystem.getInstance()) + .withTimeout(1)); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/ManualShootingPreset.java b/src/main/java/frc/robot/commands/teleop/automatic/ManualShootingPreset.java new file mode 100644 index 0000000..efc9925 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/ManualShootingPreset.java @@ -0,0 +1,13 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.commands.teleop.ToggleShooter; + +public class ManualShootingPreset extends ParallelCommandGroup { + public ManualShootingPreset(double rpm, double launchAngle, boolean autoAim) { + addCommands( + new ToggleShooter(rpm), + // new SetHoodLaunchAngle(launchAngle), // TODO fix + autoAim ? new AutoAimUsingTurret() : new ResetTurret()); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/ResetHoodDumb.java b/src/main/java/frc/robot/commands/teleop/automatic/ResetHoodDumb.java new file mode 100644 index 0000000..f0fe67d --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/ResetHoodDumb.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.teleop.automatic; + +// import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.subsystems.AdjustableHoodSubsystem; + +public class ResetHoodDumb extends SequentialCommandGroup { + public ResetHoodDumb() { // TODO this shouldnt be a thing i think + addCommands( + new StartEndCommand( + () -> AdjustableHoodSubsystem.getInstance(), + // .motor + // .getMotorController() + // .set(ControlMode.PercentOutput, 0.15), + () -> AdjustableHoodSubsystem.getInstance(), + // .motor + // .getMotorController() + // .set(ControlMode.PercentOutput, 0), + AdjustableHoodSubsystem.getInstance()) + .withTimeout(1)); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/ResetTurret.java b/src/main/java/frc/robot/commands/teleop/automatic/ResetTurret.java new file mode 100644 index 0000000..7d23efc --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/ResetTurret.java @@ -0,0 +1,10 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.TurretSubsystem; + +public class ResetTurret extends InstantCommand { + public ResetTurret() { + super(() -> TurretSubsystem.getInstance().returnToStart(), TurretSubsystem.getInstance()); + } +} diff --git a/src/main/java/frc/robot/commands/teleop/automatic/SetHoodLaunchAngle.java b/src/main/java/frc/robot/commands/teleop/automatic/SetHoodLaunchAngle.java new file mode 100644 index 0000000..dbf33d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/automatic/SetHoodLaunchAngle.java @@ -0,0 +1,12 @@ +package frc.robot.commands.teleop.automatic; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.AdjustableHoodSubsystem; + +public class SetHoodLaunchAngle extends InstantCommand { + public SetHoodLaunchAngle(double angle) { // TODO fix + super( + () -> AdjustableHoodSubsystem.getInstance(), // .setGoalLaunchAngle(angle), + AdjustableHoodSubsystem.getInstance()); + } +} diff --git a/src/main/java/frc/robot/subsystems/AdjustableHoodSubsystem.java b/src/main/java/frc/robot/subsystems/AdjustableHoodSubsystem.java new file mode 100644 index 0000000..3c9ac36 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AdjustableHoodSubsystem.java @@ -0,0 +1,149 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Pro775; + +public class AdjustableHoodSubsystem + extends SubsystemBase { // TODO hood is completely broken in some way, come back to this after + // the event + + private final boolean encoderGlitchDetection = false; + private final boolean encoderGlitchRecovery = + false; // resets the encoder and pretends like nothing ever happened, may + // work first try, may destroy robot idk + + /* + * Note that "angle" means how elevated the shot is from horizontal. An angle of + * 0 is a flat shot, 45 is a diagonal shot, and 90 is straight up. + */ + public static final double minLaunchAngle = 49; + public static final double maxLaunchAngle = 76; + private double startAngle = maxLaunchAngle; + private double goalLaunchAngle = startAngle; // setpoint + private double lastCurrentAngle = startAngle; + + private final double slowDownWithinThisAngleFromGoal = 6.0; + private final double stopWithinThisAngleFromGoal = 1.0; + private final double minimumPercentOutput = 0.15; + private final double maximumPercentOutput = 0.2; + private final double workingWithGravityAdjustment = 0.75; + + private final double encoderBrokenTime = 1.0; // seconds + private final double postBrokenRecoveryTime = + 1.0; // even though it's the same amount of time, it's working with gravity + // on the way down + + private final double sensorGearRatio = 15.0 / 72.0; + public Pro775 motor = new Pro775(RobotMap.CAN.adjustableHood, 1.0); + + // private final Timer constantAngleTimer = new Timer(); + private double beforeLastCurrentAngle = lastCurrentAngle; + + private boolean encoderBroken = false; + private boolean currentlyRecovering = false; + + private AdjustableHoodSubsystem() { + motor.getMotorController().configFactoryDefault(); + motor.getMotorController().setNeutralMode(NeutralMode.Coast); + + motor.setSensorGearRatio(sensorGearRatio); + motor.resetSensorPosition(); + + // constantAngleTimer.start(); + } + + public void setGoalLaunchAngle(double newGoalLaunchAngle) { + // goalLaunchAngle = MathUtil.clamp(newGoalLaunchAngle, minLaunchAngle, maxLaunchAngle); + } + + public boolean atGoalLaunchAngle() { + return true; + // return Math.abs(goalLaunchAngle - lastCurrentAngle) + // < slowDownWithinThisAngleFromGoal * minimumPercentOutput; + } + + public double getCurrentLaunchAngle() { + double tentativeCurrentAngle = startAngle + motor.getOutputRevsCompleted() * 360.0; + + SmartDashboard.putBoolean( + "Hood In Bounds", + tentativeCurrentAngle >= (minLaunchAngle - stopWithinThisAngleFromGoal) + && tentativeCurrentAngle <= (maxLaunchAngle + stopWithinThisAngleFromGoal)); + + if (tentativeCurrentAngle < minLaunchAngle - slowDownWithinThisAngleFromGoal + || tentativeCurrentAngle > maxLaunchAngle + slowDownWithinThisAngleFromGoal) { + return lastCurrentAngle; + } else { + lastCurrentAngle = tentativeCurrentAngle; + return lastCurrentAngle; + } + } + + @Override + public void periodic() { + // double currentLaunchAngle = getCurrentLaunchAngle(); + // double angleError = goalLaunchAngle - currentLaunchAngle; + + // double x = + // Math.min( + // (Math.abs(angleError) - stopWithinThisAngleFromGoal) + // / (slowDownWithinThisAngleFromGoal - stopWithinThisAngleFromGoal), + // 1.0); + // double motorOutput = (x * maximumPercentOutput) + ((1 - x) * minimumPercentOutput); + // if (motorOutput < minimumPercentOutput) { + // motorOutput = 0; + // } + // motorOutput *= Math.signum(angleError); + + // if (angleError > 0) { + // motorOutput *= workingWithGravityAdjustment; + // } + + // if (encoderGlitchDetection) { + // if (!encoderBroken) { + // if (Math.abs(motorOutput) > 0) { + // if (lastCurrentAngle != beforeLastCurrentAngle) { + // beforeLastCurrentAngle = lastCurrentAngle; + // constantAngleTimer.reset(); + // } else if (constantAngleTimer.get() > encoderBrokenTime) { + // encoderBroken = true; + // currentlyRecovering = true; + // motor.setRawOutput(maximumPercentOutput); // positive goes towards max angle + // } + // } else { + // constantAngleTimer.reset(); + // beforeLastCurrentAngle = lastCurrentAngle; + // } + // } else if (currentlyRecovering) { + // if (constantAngleTimer.get() > encoderBrokenTime + postBrokenRecoveryTime) { + // motor.setRawOutput(0); + // currentlyRecovering = false; + // constantAngleTimer.reset(); + + // if (encoderGlitchRecovery) { + // encoderBroken = false; + // motor.resetSensorPosition(); + // } + // } + // } + // } + + SmartDashboard.putNumber("Hood Current Angle", getCurrentLaunchAngle()); + SmartDashboard.putNumber("Hood Goal Angle", goalLaunchAngle); + SmartDashboard.putNumber("Hood Error Angle", 0.0); + SmartDashboard.putBoolean("Hood At Goal", atGoalLaunchAngle()); + SmartDashboard.putNumber("Hood Motor Output", 0.0); + SmartDashboard.putBoolean("Hood Encoder Working", !encoderBroken); + + // if (!encoderBroken) motor.setRawOutput(motorOutput); + } + + private static AdjustableHoodSubsystem instance = new AdjustableHoodSubsystem(); + + public static AdjustableHoodSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java new file mode 100644 index 0000000..66c7533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Falcon500; + +public class ClimbSubsystem extends SubsystemBase { + + private final double percentOutput = 0.75; + + private final Falcon500 climber = new Falcon500(RobotMap.CAN.climber, 8.0 / 72.0 * 18.0 / 84.0); + + private ClimbSubsystem() { + climber.getMotorController().configFactoryDefault(); + climber.getMotorController().setInverted(true); + climber.getMotorController().setNeutralMode(NeutralMode.Brake); + climber.getMotorController().configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + climber.resetSensorPosition(); + } + + public void set(int d) { + climber.setRawOutput(percentOutput * d); + } + + @Override + public void periodic() { + double current = Robot.pdp.getCurrent(RobotMap.PDP.climber); + double encoderPos = + climber.getOutputRevsCompleted() * RobotMap.Dimensions.climberWinchCircumference; + + SmartDashboard.putNumber("Climber Current", current); + SmartDashboard.putNumber("Climber Position", encoderPos); + } + + private static ClimbSubsystem instance = new ClimbSubsystem(); + + public static ClimbSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java new file mode 100644 index 0000000..c728ee5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.config.DriveConfig; +import org.awtybots.frc.botplus.motors.Falcon500; +import org.awtybots.frc.botplus.subsystems.Drivetrain; + +public class DrivetrainSubsystem extends Drivetrain { + + private static double gearRatio = 12.0 / 40.0 * 14.0 / 44.0; + private static DriveConfig driveConfig = + new DriveConfig( + true, // invertRight + 0.154, // wheelDiameter (m) + 0.03, // p + 0.0, // i + 0.0, // d + 0.07, // f + 3.0, // percentRamp + 1.0, // percentPeak (max output) + 0.07, // percentNominal (min output) + 4.0 // velocityPeak (m/s) + ); + + private DrivetrainSubsystem() { + super( + driveConfig, + new Falcon500[] { + new Falcon500(RobotMap.CAN.leftDrive1, gearRatio), + new Falcon500(RobotMap.CAN.leftDrive2, gearRatio) + }, + new Falcon500[] { + new Falcon500(RobotMap.CAN.rightDrive1, gearRatio), + new Falcon500(RobotMap.CAN.rightDrive2, gearRatio) + }); + + kill(); + } + + @Override + public void periodic() { + super.periodic(); + + SmartDashboard.putNumber("Drive Goal Output", getLeftMotors().getGoalVelocity()); + SmartDashboard.putNumber("Drive Output", getLeftMotors().getWheelVelocity()); + } + + private static DrivetrainSubsystem instance = new DrivetrainSubsystem(); + + public static DrivetrainSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index cd5a7e0..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,18 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - - public ExampleSubsystem() {} - - @Override - public void periodic() {} -} diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java new file mode 100644 index 0000000..bc62779 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Bag; + +public class IndexerSubsystem extends SubsystemBase { + + // running them at different speeds worked best last year + public static final double indexerLPercentOutput = 0.8; + public static final double indexerRPercentOutput = 0.6; + + private Bag indexerL; + private Bag indexerR; + + private IndexerSubsystem() { + indexerL = new Bag(RobotMap.CAN.indexerL, 1.0); + indexerR = new Bag(RobotMap.CAN.indexerR, 1.0); + + indexerL.getMotorController().configFactoryDefault(); + indexerR.getMotorController().configFactoryDefault(); + + indexerR.getMotorController().setInverted(true); // invert one of them + + set(0); + } + + public void set(int p) { + indexerL.setRawOutput(indexerLPercentOutput * p); + indexerR.setRawOutput(indexerRPercentOutput * p); + } + + private static IndexerSubsystem instance = new IndexerSubsystem(); + + public static IndexerSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..b35641b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Pro775; + +public class IntakeSubsystem extends SubsystemBase { + + private final double motorPercentOutput = 0.6; + + private final Pro775 motor = new Pro775(RobotMap.CAN.intake, 1.0); + private final DoubleSolenoid pistons = + new DoubleSolenoid(RobotMap.PCM.intakeFwd, RobotMap.PCM.intakeRev); + + private IntakeSubsystem() { + motor.getMotorController().configFactoryDefault(); + + toggle(false); + } + + public void toggle(boolean on) { + pistons.set(on ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse); + toggleMotorOnly(on); + } + + public void toggleMotorOnly(boolean on) { + motor.setRawOutput(on ? motorPercentOutput : 0); + } + + public void reverse() { + motor.setRawOutput(-motorPercentOutput); + } + + private static IntakeSubsystem instance = new IntakeSubsystem(); + + public static IntakeSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..692831e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,97 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.math.Flywheel; +import org.awtybots.frc.botplus.motors.Falcon500; + +public class ShooterSubsystem extends SubsystemBase { + + private Falcon500 motor = new Falcon500(RobotMap.CAN.shooter, 1.0); + private final double maxRevsPerSecondError = 1.0; + public double goalRevsPerSecond = 0.0; + public Flywheel flywheel = + new Flywheel( + 0.051, // flywheel radius (m) + motor, + 0.9 // efficiency factor + ); + + private double kP = 0.1; + private double kI = 0; + private double kD = 0; + private double kF = 0.044; + + private boolean pidfChanged = false; + private boolean flywheelReady = true; + + private ShooterSubsystem() { + motor.getMotorController().setSensorPhase(true); + motor.getMotorController().configClosedloopRamp(2.0); + motor.getMotorController().setNeutralMode(NeutralMode.Coast); + motor.getMotorController().configVoltageCompSaturation(13.0); + motor.getMotorController().enableVoltageCompensation(true); + + motor.setPIDF(kP, kI, kD, kF); + + if (Robot.Companion.getTestMode()) { + SmartDashboard.putNumber("Shooter PID - P", kP); + SmartDashboard.putNumber("Shooter PID - I", kI); + SmartDashboard.putNumber("Shooter PID - D", kD); + SmartDashboard.putNumber("Shooter PID - F", kF); + } + } + + public void setFlywheelRevsPerSecond(double rps) { + SmartDashboard.putNumber("Shooter Goal RPM", rps * 60.0); + goalRevsPerSecond = rps; + motor.setRevsPerSecond(rps); + } + + public void stopFlywheel() { + setFlywheelRevsPerSecond(0.0); + } + + public boolean isFlywheelReady() { + return flywheelReady; + } + + @Override + public void periodic() { + if (Robot.Companion.getTestMode()) { + pidfChanged = false; + + kP = getNumberFromSmartDashboard("Shooter PID - P", kP); + kI = getNumberFromSmartDashboard("Shooter PID - I", kI); + kD = getNumberFromSmartDashboard("Shooter PID - D", kD); + kF = getNumberFromSmartDashboard("Shooter PID - F", kF); + + if (pidfChanged) motor.setPIDF(kP, kI, kD, kF); + } + + double outputRps = motor.getOutputRevsPerSecond(); + double outputRpm = outputRps * 60.0; + flywheelReady = Math.abs(goalRevsPerSecond - outputRps) < maxRevsPerSecondError; + + SmartDashboard.putNumber("Shooter Actual RPM", outputRpm); + SmartDashboard.putNumber("Shooter Error RPM", Math.abs(outputRpm - (goalRevsPerSecond * 60.0))); + SmartDashboard.putBoolean("Shooter At Goal", flywheelReady); + } + + private double getNumberFromSmartDashboard(String name, double previousValue) { + double newValue = SmartDashboard.getNumber(name, previousValue); + if (newValue != previousValue) { + pidfChanged = true; + } + return newValue; + } + + private static ShooterSubsystem instance = new ShooterSubsystem(); + + public static ShooterSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/TowerSubsystem.java b/src/main/java/frc/robot/subsystems/TowerSubsystem.java new file mode 100644 index 0000000..50224ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TowerSubsystem.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Pro775; + +public class TowerSubsystem extends SubsystemBase { + + private static final double loadingPercentOutput = 0.25; // slow while loading balls + private static final double shootingPercentOutput = 0.65; // fast while shooting + private static final double reversePercentOutput = -0.3; + + private final Pro775 tower = new Pro775(RobotMap.CAN.tower, 1.0); + private final DigitalInput limitSwitch = new DigitalInput(RobotMap.DIO.towerLimitSwitch); + + private boolean + stopIfLimitSwitchPressed; // this is not a setting, this is just a flag for internal use + + private TowerSubsystem() { + tower.getMotorController().configFactoryDefault(); + tower.getMotorController().setInverted(false); + + stop(); + } + + public void enableForLoading() { + if (isFull()) return; + + tower.setRawOutput(loadingPercentOutput); + stopIfLimitSwitchPressed = true; + } + + public void enableForShooting() { + tower.setRawOutput(shootingPercentOutput); + stopIfLimitSwitchPressed = false; + } + + public void reverse() { + tower.setRawOutput(reversePercentOutput); + stopIfLimitSwitchPressed = false; + } + + public void stop() { + tower.setRawOutput(0.0); + stopIfLimitSwitchPressed = false; + } + + /** Determine if a ball pressing the limit switch, indicating if the tower is full. */ + private boolean isFull() { + return limitSwitch.get(); + } + + @Override + public void periodic() { + if (stopIfLimitSwitchPressed && isFull()) { + stop(); + } + } + + private static TowerSubsystem instance = new TowerSubsystem(); + + public static TowerSubsystem getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java new file mode 100644 index 0000000..c5c5fb2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -0,0 +1,112 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpiutil.math.MathUtil; +import frc.robot.RobotMap; +import org.awtybots.frc.botplus.motors.Pro775; + +public class TurretSubsystem extends SubsystemBase { + + public static final double minAngle = 135; + public static final double maxAngle = 225; + public static final double startAngle = 180; + private double goalAngle = startAngle; // goalAngle is setpoint + private double lastCurrentAngle = startAngle; + + private double slowDownWithinThisAngleFromGoal = 20.0; + private double stopWithinThisAngleFromGoal = 3.0; + private double minimumPercentOutput = 0.13; + private double maximumPercentOutput = 0.23; + + private final double sensorGearRatio = + 22.0 / 240.0; // ratio between actual output rotation and encoder-detected + // rotation + private Pro775 motor; + + private TurretSubsystem() { + motor = new Pro775(RobotMap.CAN.turret, 1.0); + + motor.getMotorController().configFactoryDefault(); + motor.getMotorController().setNeutralMode(NeutralMode.Coast); + + motor.setSensorGearRatio(sensorGearRatio); + motor.resetSensorPosition(); + + motor.getMotorController().setInverted(true); + motor.getMotorController().setSensorPhase(true); + } + + public void setAbsoluteGoalAngle(double newGoalAngle) { + newGoalAngle %= 360.0; + goalAngle = MathUtil.clamp(newGoalAngle, minAngle, maxAngle); + + boolean goalReachable = newGoalAngle >= minAngle && newGoalAngle < maxAngle; + SmartDashboard.putBoolean("Turret Goal Reachable", goalReachable); + } + + public void setRelativeGoalAngle(double newGoalAngle) { + setAbsoluteGoalAngle(lastCurrentAngle + newGoalAngle); + } + + public boolean atGoalAngle() { + return Math.abs(goalAngle - lastCurrentAngle) < stopWithinThisAngleFromGoal; + } + + public double getCurrentAngle() { + double tentativeCurrentAngle = startAngle + motor.getOutputRevsCompleted() * 360.0; + + SmartDashboard.putBoolean( + "Turret In Bounds", + tentativeCurrentAngle >= (minAngle - stopWithinThisAngleFromGoal) + && tentativeCurrentAngle <= (maxAngle + stopWithinThisAngleFromGoal)); + + if (tentativeCurrentAngle < minAngle - slowDownWithinThisAngleFromGoal + || tentativeCurrentAngle > maxAngle + slowDownWithinThisAngleFromGoal) { + return lastCurrentAngle; + } else { + lastCurrentAngle = tentativeCurrentAngle; + return lastCurrentAngle; + } + } + + public void relax() { + goalAngle = lastCurrentAngle; + } + + public void returnToStart() { + goalAngle = startAngle; + } + + @Override + public void periodic() { + double currentAngle = getCurrentAngle(); + double angleError = goalAngle - currentAngle; + + SmartDashboard.putNumber("Turret Current Angle", currentAngle); + SmartDashboard.putNumber("Turret Goal Angle", goalAngle); + SmartDashboard.putNumber("Turret Error Angle", angleError); + SmartDashboard.putBoolean("Turret At Goal", atGoalAngle()); + + double x = + Math.min( + (Math.abs(angleError) - stopWithinThisAngleFromGoal) + / (slowDownWithinThisAngleFromGoal - stopWithinThisAngleFromGoal), + 1.0); + double motorOutput = (x * maximumPercentOutput) + ((1 - x) * minimumPercentOutput); + if (motorOutput < minimumPercentOutput) { + motorOutput = 0; + } + motorOutput *= Math.signum(angleError); + + SmartDashboard.putNumber("Turret Motor Output", motorOutput); + motor.setRawOutput(motorOutput); + } + + private static TurretSubsystem instance = new TurretSubsystem(); + + public static TurretSubsystem getInstance() { + return instance; + } +}