├── src └── main │ ├── python │ ├── paths │ │ └── test.json │ ├── requirements.txt │ ├── drive │ │ ├── __pycache__ │ │ │ ├── swerve_drive.cpython-38.pyc │ │ │ └── swerve_drive.cpython-39.pyc │ │ ├── diff_drive.py │ │ └── swerve_drive.py │ ├── trajectory_io.py │ ├── trajectory_util.py │ ├── trajectory_generator.py │ └── paths.py │ ├── deploy │ └── images │ │ ├── burst.bmp │ │ ├── fade.png │ │ ├── noise.png │ │ ├── pulse.png │ │ ├── THfade.png │ │ ├── stripe.png │ │ ├── stripes.png │ │ ├── pulse-down.png │ │ ├── noisy-pulse.png │ │ └── yellow_stripes.png │ └── java │ └── frc │ ├── lib │ ├── control │ │ ├── SwerveFeedforwards.java │ │ ├── SwerveTrajectoryFollower.java │ │ ├── DCMotor.java │ │ ├── Vector3d.java │ │ ├── PIDController.java │ │ └── SwerveTrajectory.java │ ├── HelixJoysticks.java │ ├── util │ │ ├── InterpolatingPoseMap.java │ │ └── InterpolationTable.java │ ├── LinCurve.java │ ├── ControllerPatroller.java │ └── Curve.java │ ├── paths │ ├── Path.java │ ├── main.java │ ├── FiveBallPartOne.java │ ├── ShootTwoBalls.java │ ├── gogogogadget.java │ ├── OnePointEightMetersForward.java │ ├── FiveBallPartThree.java │ └── NewAutoPartThree.java │ └── robot │ ├── vision │ ├── Photon.java │ ├── commands │ │ ├── TurnOnLEDs.java │ │ └── TurnOffLEDs.java │ └── Limelight.java │ ├── drive │ └── commands │ │ ├── Test.java │ │ ├── ResetEncoders.java │ │ ├── AbsoluteOrientation.java │ │ ├── ResetOdometry.java │ │ ├── RelativeOrientation.java │ │ ├── ZeroHeading.java │ │ ├── JoystickDrive.java │ │ ├── Drive.java │ │ ├── TestDrive.java │ │ ├── TurnToAngle.java │ │ ├── TrajectoryFollower.java │ │ └── MotionProfileTurn.java │ ├── shooter │ └── commands │ │ ├── EjectTrigger.java │ │ ├── ResetEncoder.java │ │ ├── StopTrigger.java │ │ ├── PullTrigger.java │ │ ├── ResetHood.java │ │ ├── SetShooterState.java │ │ ├── StopShooter.java │ │ ├── VisionAutoShooter.java │ │ ├── SpinUpShooter.java │ │ ├── MoveHoodJoystick.java │ │ ├── MoveHoodButton.java │ │ ├── PresetFlywheelController.java │ │ ├── FlywheelController.java │ │ └── VisionShooter.java │ ├── climber │ ├── commands │ │ ├── DeployClimber.java │ │ ├── RetractClimber.java │ │ └── ToggleClimber.java │ └── Climber.java │ ├── intake │ ├── commands │ │ ├── EjectIntake.java │ │ ├── SpinIntake.java │ │ ├── OpenIntake.java │ │ ├── FastIntake.java │ │ └── RetractIntake.java │ └── Intake.java │ ├── Main.java │ ├── status │ ├── actions │ │ ├── Action.java │ │ ├── RainbowAction.java │ │ ├── ChaseAction.java │ │ ├── PowerUpAction.java │ │ ├── LedAction.java │ │ └── ScannerAction.java │ ├── commands │ │ ├── FillLEDsCommand.java │ │ ├── XBoxButtonCommand.java │ │ ├── SetColor.java │ │ ├── ActionCommand.java │ │ ├── DIOSwitchStatus.java │ │ └── IdleCommand.java │ └── groups │ │ └── LEDDemoCG.java │ └── auto │ └── groups │ ├── ShootAndDriveForward.java │ ├── SuperRudeAuto.java │ ├── SecondSuperRudeAuto.java │ ├── TwoBallEastAuto.java │ ├── TwoBallSouthAuto.java │ ├── NewFourBallAuto.java │ ├── FiveBallAuto.java │ ├── FourBallAuto.java │ └── NewFiveBallAuto.java ├── gradlew ├── animations └── trajectory.gif ├── RadioMaster OTX └── Swerve model.otx ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── .wpilib └── wpilib_preferences.json ├── networktables.ini.bak ├── settings.gradle ├── vendordeps ├── ADIS16470.json.xxx ├── navx_frc.json ├── WPILibNewCommands.json ├── photonlib.json └── REVLib.json ├── LICENSE ├── simgui-ds.json ├── gradlew.bat └── .gitignore /src/main/python/paths/test.json: -------------------------------------------------------------------------------- 1 | [ 2 | [1,2,3], 3 | [2,3,4] 4 | ] -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/gradlew -------------------------------------------------------------------------------- /animations/trajectory.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/animations/trajectory.gif -------------------------------------------------------------------------------- /RadioMaster OTX/Swerve model.otx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/RadioMaster OTX/Swerve model.otx -------------------------------------------------------------------------------- /src/main/deploy/images/burst.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/burst.bmp -------------------------------------------------------------------------------- /src/main/deploy/images/fade.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/fade.png -------------------------------------------------------------------------------- /src/main/deploy/images/noise.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/noise.png -------------------------------------------------------------------------------- /src/main/deploy/images/pulse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/pulse.png -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/SwerveFeedforwards.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | public class SwerveFeedforwards { 4 | 5 | } -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /src/main/deploy/images/THfade.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/THfade.png -------------------------------------------------------------------------------- /src/main/deploy/images/stripe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/stripe.png -------------------------------------------------------------------------------- /src/main/deploy/images/stripes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/stripes.png -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/SwerveTrajectoryFollower.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | public class SwerveTrajectoryFollower { 4 | } -------------------------------------------------------------------------------- /src/main/deploy/images/pulse-down.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/pulse-down.png -------------------------------------------------------------------------------- /src/main/python/requirements.txt: -------------------------------------------------------------------------------- 1 | PyQt5==5.15.4 2 | casadi~=3.5.5 3 | matplotlib~=3.3.3 4 | numpy~=1.19.3 5 | pillow~=8.2.0 6 | qtmodern~=0.2.0 -------------------------------------------------------------------------------- /src/main/deploy/images/noisy-pulse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/noisy-pulse.png -------------------------------------------------------------------------------- /src/main/deploy/images/yellow_stripes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/deploy/images/yellow_stripes.png -------------------------------------------------------------------------------- /.wpilib/wpilib_preferences.json: -------------------------------------------------------------------------------- 1 | { 2 | "enableCppIntellisense": false, 3 | "currentLanguage": "java", 4 | "projectYear": "2022", 5 | "teamNumber": 2363 6 | } -------------------------------------------------------------------------------- /src/main/python/drive/__pycache__/swerve_drive.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/python/drive/__pycache__/swerve_drive.cpython-38.pyc -------------------------------------------------------------------------------- /src/main/python/drive/__pycache__/swerve_drive.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TripleHelixProgramming/RapidReact/HEAD/src/main/python/drive/__pycache__/swerve_drive.cpython-39.pyc -------------------------------------------------------------------------------- /src/main/java/frc/paths/Path.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public abstract class Path { 6 | public abstract SwerveTrajectory getPath(); 7 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/vision/Photon.java: -------------------------------------------------------------------------------- 1 | package frc.robot.vision; 2 | 3 | import org.photonvision.PhotonCamera; 4 | 5 | public class Photon { 6 | PhotonCamera camera = new PhotonCamera("photonvision/limelight"); 7 | 8 | 9 | } -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/Test.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.controller.ProfiledPIDController; 4 | import edu.wpi.first.math.trajectory.TrapezoidProfile; 5 | 6 | public class Test { 7 | public static void main(String[] args) { 8 | ProfiledPIDController controller = new ProfiledPIDController(1,0,0,new TrapezoidProfile.Constraints(1, 1)); 9 | System.out.println(controller.calculate(1, 2)); 10 | 11 | } 12 | } -------------------------------------------------------------------------------- /src/main/java/frc/paths/main.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.PIDController; 4 | import frc.lib.control.SwerveTrajectory; 5 | 6 | public class main { 7 | public static void main(String[] args) { 8 | PIDController controller = new PIDController(1, 0, 0); 9 | controller.setContinous(true); 10 | controller.setInputRange(360); 11 | 12 | controller.setReference(-790); 13 | System.out.println(controller.calculate(130, 1)); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /networktables.ini.bak: -------------------------------------------------------------------------------- 1 | [NetworkTables Storage 3.0] 2 | string "/Preferences/.type"="RobotPreferences" 3 | double "/Preferences/BUF.Angle"=83 4 | double "/Preferences/BUF.Velocity"=1710 5 | double "/Preferences/BUR.Angle"=74.1 6 | double "/Preferences/BUR.Velocity"=1860 7 | double "/Preferences/SAF.Angle"=71 8 | double "/Preferences/SAF.Velocity"=1990 9 | double "/Preferences/TLR.Angle"=110 10 | double "/Preferences/TLR.Velocity"=1000 11 | double "/Preferences/TUR.Angle"=105 12 | double "/Preferences/TUR.Velocity"=1400 13 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/EjectTrigger.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.shooter.Shooter; 5 | 6 | public class EjectTrigger extends CommandBase { 7 | Shooter shooter; 8 | 9 | public EjectTrigger(Shooter shooter) { 10 | this.shooter = shooter; 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | shooter.reverseTrigger(); 16 | } 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/climber/commands/DeployClimber.java: -------------------------------------------------------------------------------- 1 | package frc.robot.climber.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.climber.Climber; 5 | 6 | public class DeployClimber extends CommandBase { 7 | private Climber climber; 8 | public DeployClimber(Climber climber) { 9 | this.climber = climber; 10 | addRequirements(climber); 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | climber.deploy(); 16 | } 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/climber/commands/RetractClimber.java: -------------------------------------------------------------------------------- 1 | package frc.robot.climber.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.climber.Climber; 5 | 6 | public class RetractClimber extends CommandBase { 7 | private Climber climber; 8 | public RetractClimber(Climber climber) { 9 | this.climber = climber; 10 | addRequirements(climber); 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | climber.disable(); 16 | } 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/climber/commands/ToggleClimber.java: -------------------------------------------------------------------------------- 1 | package frc.robot.climber.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.climber.Climber; 5 | 6 | public class ToggleClimber extends CommandBase { 7 | private Climber climber; 8 | public ToggleClimber(Climber climber) { 9 | this.climber = climber; 10 | addRequirements(climber); 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | // climber.toggle(); 16 | } 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/vision/commands/TurnOnLEDs.java: -------------------------------------------------------------------------------- 1 | package frc.robot.vision.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.vision.Limelight; 5 | 6 | public class TurnOnLEDs extends CommandBase { 7 | Limelight limelight; 8 | 9 | public TurnOnLEDs(Limelight limelight) { 10 | this.limelight = limelight; 11 | addRequirements(limelight); 12 | } 13 | 14 | @Override 15 | public void execute() { 16 | limelight.turnOnLEDs(); 17 | } 18 | 19 | @Override 20 | public boolean isFinished() { 21 | return false; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/vision/commands/TurnOffLEDs.java: -------------------------------------------------------------------------------- 1 | package frc.robot.vision.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.vision.Limelight; 5 | 6 | public class TurnOffLEDs extends CommandBase { 7 | Limelight limelight; 8 | 9 | public TurnOffLEDs(Limelight limelight) { 10 | this.limelight = limelight; 11 | addRequirements(limelight); 12 | } 13 | 14 | @Override 15 | public void execute() { 16 | limelight.turnOffLEDs(); 17 | } 18 | 19 | @Override 20 | public boolean isFinished() { 21 | return false; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/ResetEncoder.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.shooter.Shooter; 5 | 6 | public class ResetEncoder extends CommandBase { 7 | Shooter shooter; 8 | 9 | public ResetEncoder(Shooter shooter) { 10 | this.shooter = shooter; 11 | } 12 | 13 | // Called when the command is initially scheduled. 14 | @Override 15 | public void initialize() { 16 | shooter.resetHoodEncoder(); 17 | } 18 | 19 | @Override 20 | public boolean isFinished() { 21 | return true; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/commands/EjectIntake.java: -------------------------------------------------------------------------------- 1 | package frc.robot.intake.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.intake.Intake; 6 | 7 | public class EjectIntake extends CommandBase { 8 | private Intake intake; 9 | 10 | public EjectIntake(Intake intake) { 11 | this.intake = intake; 12 | addRequirements(intake); 13 | } 14 | 15 | @Override 16 | public void initialize() { 17 | intake.deploy(); 18 | Timer.delay(0.2); 19 | intake.rollerOut(); 20 | } 21 | 22 | @Override 23 | public boolean isFinished() { 24 | return false; 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/commands/SpinIntake.java: -------------------------------------------------------------------------------- 1 | package frc.robot.intake.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.intake.Intake; 6 | 7 | public class SpinIntake extends CommandBase { 8 | private Intake intake; 9 | 10 | public SpinIntake(Intake intake) { 11 | this.intake = intake; 12 | addRequirements(intake); 13 | } 14 | 15 | @Override 16 | public void initialize() { 17 | // intake.deploy(); 18 | // Timer.delay(0.2); 19 | // intake.rollerPush(); 20 | } 21 | 22 | @Override 23 | public boolean isFinished() { 24 | return false; 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/DCMotor.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | public class DCMotor { 4 | private double kv, ka, ks; 5 | 6 | public DCMotor(double kv, double ka, double ks) { 7 | this.kv = kv; 8 | this.ka = ka; 9 | this.ks = ks; 10 | } 11 | 12 | public double solveFeedforward(double velocity, double acceleration) { 13 | return kv * velocity + ka * acceleration + ks; 14 | } 15 | 16 | public double solveVoltage(double initialVelocity, double finalVelocity, double dt) { 17 | double m0 = Math.pow(Math.E, -kv / ka * dt); 18 | return (initialVelocity - finalVelocity * m0) / (1 - m0) * kv; 19 | } 20 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/StopTrigger.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.intake.Intake; 5 | import frc.robot.shooter.Shooter; 6 | 7 | public class StopTrigger extends CommandBase { 8 | Shooter shooter; 9 | Intake intake; 10 | 11 | public StopTrigger(Shooter shooter, Intake intake) { 12 | this.shooter = shooter; 13 | this.intake = intake; 14 | } 15 | 16 | @Override 17 | public void initialize() { 18 | shooter.stopTrigger(); 19 | intake.setPushCargo(false); 20 | } 21 | 22 | @Override 23 | public boolean isFinished() { 24 | return true; 25 | } 26 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/PullTrigger.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.intake.Intake; 5 | import frc.robot.shooter.Shooter; 6 | 7 | public class PullTrigger extends CommandBase { 8 | Shooter shooter; 9 | Intake intake; 10 | 11 | public PullTrigger(Shooter shooter, Intake intake) { 12 | this.shooter = shooter; 13 | this.intake = intake; 14 | } 15 | 16 | @Override 17 | public void initialize() { 18 | shooter.startTrigger(); 19 | intake.setPushCargo(true); 20 | } 21 | 22 | @Override 23 | public boolean isFinished() { 24 | return true; 25 | } 26 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/Main.java: -------------------------------------------------------------------------------- 1 | package frc.robot; 2 | 3 | import edu.wpi.first.wpilibj.RobotBase; 4 | 5 | /** 6 | * Do NOT add any static variables to this class, or any initialization at all. Unless you know what 7 | * you are doing, do not modify this file except to change the parameter class to the startRobot 8 | * call. 9 | */ 10 | public final class Main { 11 | private Main() {} 12 | 13 | /** 14 | * Main initialization function. Do not perform any initialization here. 15 | * 16 | *

If you change your main robot class, change the parameter type. 17 | */ 18 | public static void main(String... args) { 19 | RobotBase.startRobot(Robot::new); 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/HelixJoysticks.java: -------------------------------------------------------------------------------- 1 | package frc.lib; 2 | 3 | import edu.wpi.first.wpilibj.Joystick; 4 | 5 | public class HelixJoysticks { 6 | private Joystick controller; 7 | private int x, y, theta; 8 | 9 | public HelixJoysticks(Joystick controller, int x, int y, int theta) { 10 | this.controller = controller; 11 | this.x = x; 12 | this.y = y; 13 | this.theta = theta; 14 | } 15 | 16 | public double getX() { 17 | return controller.getRawAxis(x); 18 | } 19 | 20 | public double getY() { 21 | return controller.getRawAxis(y); 22 | } 23 | 24 | public double getRotation() { 25 | return controller.getRawAxis(theta); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/ResetHood.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.Constants.ShooterConstants; 5 | import frc.robot.shooter.Shooter; 6 | 7 | public class ResetHood extends CommandBase { 8 | Shooter shooter; 9 | public ResetHood(Shooter shooter) { 10 | this.shooter = shooter; 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | shooter.setHoodSpeed(-ShooterConstants.kHoodSpeed); 16 | } 17 | 18 | @Override 19 | public void end(boolean interrupted) { 20 | shooter.resetHoodAngle(); 21 | shooter.stopHood(); 22 | } 23 | 24 | @Override 25 | public boolean isFinished() { 26 | return shooter.checkHoodCurrentLimit(); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/climber/Climber.java: -------------------------------------------------------------------------------- 1 | package frc.robot.climber; 2 | 3 | import edu.wpi.first.wpilibj.DoubleSolenoid; 4 | import edu.wpi.first.wpilibj.Solenoid; 5 | import edu.wpi.first.wpilibj.DoubleSolenoid.Value; 6 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 7 | import frc.robot.Constants.ElectricalConstants; 8 | 9 | public class Climber extends SubsystemBase { 10 | private DoubleSolenoid climber = new DoubleSolenoid(ElectricalConstants.kPneumaticHub, ElectricalConstants.kClimberDeployPort, ElectricalConstants.kClimberRetractPort); 11 | 12 | // public void toggle() { 13 | // boolean deployed = climber.get(); 14 | // climber.set(!deployed); 15 | // } 16 | 17 | public void deploy() { 18 | climber.set(Value.kForward); 19 | } 20 | 21 | public void disable() { 22 | climber.set(Value.kReverse); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/ResetEncoders.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.drive.Drivetrain; 5 | 6 | public class ResetEncoders extends CommandBase { 7 | // The subsystem the command runs on 8 | private final Drivetrain drivetrain; 9 | 10 | public ResetEncoders(Drivetrain subsystem){ 11 | drivetrain = subsystem; 12 | addRequirements(drivetrain); 13 | } 14 | 15 | @Override 16 | public void initialize() { 17 | } 18 | 19 | @Override 20 | public boolean runsWhenDisabled() { 21 | return true; 22 | } 23 | 24 | @Override 25 | public void execute() { 26 | drivetrain.resetEncoders(); 27 | } 28 | 29 | @Override 30 | public boolean isFinished() { 31 | return true; 32 | } 33 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/SetShooterState.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.drive.Drivetrain; 5 | import frc.robot.shooter.Shooter; 6 | 7 | public class SetShooterState extends CommandBase { 8 | private Shooter shooter; 9 | private int rpm; 10 | private double hoodAngle; 11 | 12 | public SetShooterState(Shooter shooter, int rpm, double hoodAngle) { 13 | addRequirements(shooter); 14 | this.shooter = shooter; 15 | this.rpm = rpm; 16 | this.hoodAngle = hoodAngle; 17 | } 18 | // Called when the command is initially scheduled. 19 | @Override 20 | public void initialize() { 21 | shooter.setShooterVelocity(rpm); 22 | shooter.setHoodPosition(hoodAngle); 23 | } 24 | 25 | @Override 26 | public boolean isFinished() { 27 | return true; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/AbsoluteOrientation.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Rotation2d; 4 | import edu.wpi.first.wpilibj.Joystick; 5 | import frc.lib.HelixJoysticks; 6 | import frc.robot.drive.Drivetrain; 7 | 8 | public class AbsoluteOrientation extends JoystickDrive { 9 | 10 | public AbsoluteOrientation(Drivetrain subsystem, HelixJoysticks joysticks) { 11 | super(subsystem,joysticks); 12 | } 13 | 14 | // @Override 15 | // public double getTheta() { 16 | // double thetaRaw = m_controller.getRotation(); 17 | // Rotation2d theta = Rotation2d.fromDegrees(thetaRaw * 180.0); 18 | // drivetrain.rotateAbsolute(theta); 19 | // return drivetrain.getThetaDot(); 20 | // } 21 | 22 | @Override 23 | public boolean getFieldRelative() { 24 | return false; 25 | } 26 | 27 | 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/commands/OpenIntake.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.intake.commands; 6 | 7 | import edu.wpi.first.wpilibj2.command.CommandBase; 8 | import frc.robot.intake.Intake; 9 | 10 | public class OpenIntake extends CommandBase { 11 | Intake intake; 12 | /** Creates a new OpenIntake. */ 13 | public OpenIntake(Intake intake) { 14 | this.intake = intake; 15 | addRequirements(intake); 16 | } 17 | 18 | // Called when the command is initially scheduled. 19 | @Override 20 | public void initialize() { 21 | intake.deploy(); 22 | intake.topRollerStop(); 23 | } 24 | 25 | // Returns true when the command should end. 26 | @Override 27 | public boolean isFinished() { 28 | return false; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2022' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name 'frcHome' 24 | url frcHomeMaven 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/commands/FastIntake.java: -------------------------------------------------------------------------------- 1 | package frc.robot.intake.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.intake.Intake; 6 | 7 | public class FastIntake extends CommandBase { 8 | private Intake intake; 9 | private double startTime; 10 | private boolean rollersRunning = false; 11 | 12 | public FastIntake(Intake intake) { 13 | this.intake = intake; 14 | addRequirements(intake); 15 | } 16 | 17 | @Override 18 | public void initialize() { 19 | intake.deploy(); 20 | rollersRunning = false; 21 | startTime = Timer.getFPGATimestamp(); 22 | } 23 | 24 | @Override 25 | public void execute() { 26 | double timeElapsed = Timer.getFPGATimestamp() - startTime; 27 | if (!rollersRunning && (timeElapsed >= 0.20)) { 28 | intake.fastRollerIn(); 29 | rollersRunning = true; 30 | } 31 | } 32 | 33 | @Override 34 | public boolean isFinished() { 35 | return false; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/StopShooter.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.shooter.Shooter; 5 | 6 | public class StopShooter extends CommandBase { 7 | private Shooter shooter; 8 | /** Creates a new ShooterStop. */ 9 | public StopShooter(Shooter shooter) { 10 | this.shooter = shooter; 11 | addRequirements(this.shooter); 12 | // Use addRequirements() here to declare subsystem dependencies. 13 | } 14 | 15 | // Called when the command is initially scheduled. 16 | @Override 17 | public void initialize() { 18 | shooter.stopShooter(); 19 | } 20 | 21 | // Called every time the scheduler runs while the command is scheduled. 22 | @Override 23 | public void execute() {} 24 | 25 | // Called once the command ends or is interrupted. 26 | @Override 27 | public void end(boolean interrupted) {} 28 | 29 | // Returns true when the command should end. 30 | @Override 31 | public boolean isFinished() { 32 | return true; 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/commands/RetractIntake.java: -------------------------------------------------------------------------------- 1 | package frc.robot.intake.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.intake.Intake; 6 | 7 | public class RetractIntake extends CommandBase { 8 | private Intake intake; 9 | private double startRetractTime; 10 | private boolean topStopped = false; 11 | 12 | public RetractIntake(Intake intake) { 13 | this.intake = intake; 14 | addRequirements(intake); 15 | } 16 | 17 | @Override 18 | public void initialize() { 19 | intake.retract(); 20 | intake.bottomRollerStop(); 21 | topStopped = false; 22 | startRetractTime = Timer.getFPGATimestamp(); 23 | } 24 | 25 | @Override 26 | public void execute() { 27 | double timeElapsed = Timer.getFPGATimestamp() - startRetractTime; 28 | if (!topStopped && (timeElapsed >= 0.75)) { 29 | intake.topRollerStop(); 30 | topStopped = true; 31 | } 32 | } 33 | 34 | @Override 35 | public boolean isFinished() { 36 | return false; 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /vendordeps/ADIS16470.json.xxx: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "ADIS16470.json", 3 | "name": "ADIS16470-IMU", 4 | "version": "2020.r4", 5 | "uuid": "6606dc7e-fb96-436f-a804-c07a5d841a14", 6 | "allowInsecureProtocol": true, 7 | "mavenUrls": [ 8 | "http://maven.highcurrent.io/maven" 9 | ], 10 | "jsonUrl": "http://maven.highcurrent.io/vendordeps/ADIS16470.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "com.github.juchong", 14 | "artifactId": "adis16470-java", 15 | "version": "2020.r4" 16 | } 17 | ], 18 | "jniDependencies": [], 19 | "cppDependencies": [ 20 | { 21 | "groupId": "com.github.juchong", 22 | "artifactId": "adis16470-cpp", 23 | "version": "2020.r4", 24 | "libName": "libadis16470imu", 25 | "headerClassifier": "headers", 26 | "sharedLibrary": false, 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "linuxathena" 30 | ] 31 | } 32 | ] 33 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/Action.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | import frc.robot.status.Status; 4 | 5 | public class Action implements Runnable { 6 | Status status; 7 | // How long to delay intervals. 8 | protected double intervalTime = 0.0; 9 | 10 | public double getIntervalTime() { 11 | return intervalTime; 12 | } 13 | 14 | public void setIntervalTime(double intervalTime) { 15 | this.intervalTime = intervalTime; 16 | } 17 | 18 | // If this returns true the action runner thread will stop invoking run on this action. 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | 23 | // How long to delay the action runner thread before calling run again. 24 | // The thread will have a minium that is enforced if this value is to low. 25 | 26 | // Reset the action. Used when adding to the action runner to reset anything that needs resetting. 27 | public void reset() {} 28 | 29 | // Every loop of the action runner thread will call this method. 30 | public void run() {} 31 | 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/ResetOdometry.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.drive.Drivetrain; 6 | 7 | public class ResetOdometry extends CommandBase { 8 | private Pose2d pose; 9 | private Drivetrain drive; 10 | /** Creates a new ResetOdometry. */ 11 | public ResetOdometry(Drivetrain drive, Pose2d pose) { 12 | this.drive = drive; 13 | this.pose = pose; 14 | } 15 | 16 | // Called when the command is initially scheduled. 17 | @Override 18 | public void initialize() { 19 | drive.resetOdometry(pose.getRotation().getDegrees(), pose); 20 | } 21 | 22 | // Called every time the scheduler runs while the command is scheduled. 23 | @Override 24 | public void execute() {} 25 | 26 | // Called once the command ends or is interrupted. 27 | @Override 28 | public void end(boolean interrupted) {} 29 | 30 | // Returns true when the command should end. 31 | @Override 32 | public boolean isFinished() { 33 | return true; 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/RainbowAction.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | public class RainbowAction extends LedAction { 4 | 5 | // This member tracks the hue between updateBuffer calls. 6 | private int rainbowHue = 0; 7 | 8 | // Default will run a rainbow pattern. 9 | public RainbowAction() { 10 | super(); 11 | 12 | // Run forever, at 50ms between iterations. 13 | intervalCount = -1; 14 | intervalTime = 0.050; 15 | } 16 | 17 | protected void updateBuffer() { 18 | 19 | for (var i = 0; i < buffer.getLength(); i++) { 20 | 21 | // Calculate the hue - hue is easier for rainbows because the color 22 | // shape is a circle so only one value needs to precess 23 | final var hue = (rainbowHue + (i * 180 / buffer.getLength())) % 180; 24 | 25 | // Set the value 26 | buffer.setHSV(i, hue, 255, 128); 27 | } 28 | // Increase by to make the rainbow "move" 29 | rainbowHue += 3; 30 | 31 | // Check bounds 32 | rainbowHue %= 180; 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/Vector3d.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Translation2d; 6 | 7 | public class Vector3d { 8 | public final double x, y, z; 9 | 10 | public Vector3d(double x, double y, double z) { 11 | this.x = x; 12 | this.y = y; 13 | this.z = z; 14 | } 15 | 16 | public static Vector3d fromPose(Pose2d pose) { 17 | return new Vector3d(pose.getX(), pose.getY(), pose.getRotation().getRadians()); 18 | } 19 | 20 | public Pose2d toPose() { 21 | return new Pose2d(new Translation2d(x, y), new Rotation2d(z)); 22 | } 23 | 24 | public Vector3d plus(Vector3d vector) { 25 | return new Vector3d(x + vector.x, y + vector.y, z + vector.z); 26 | } 27 | 28 | public Vector3d minus(Vector3d vector) { 29 | return new Vector3d(x - vector.x, y - vector.y, z - vector.z); 30 | } 31 | 32 | public Vector3d times(double scale) { 33 | return new Vector3d(x * scale, y * scale, z * scale); 34 | } 35 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Triple Helix Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /vendordeps/navx_frc.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "navx_frc.json", 3 | "name": "KauaiLabs_navX_FRC", 4 | "version": "4.0.442", 5 | "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", 6 | "mavenUrls": [ 7 | "https://repo1.maven.org/maven2/" 8 | ], 9 | "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.kauailabs.navx.frc", 13 | "artifactId": "navx-java", 14 | "version": "4.0.442" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [ 19 | { 20 | "groupId": "com.kauailabs.navx.frc", 21 | "artifactId": "navx-cpp", 22 | "version": "4.0.442", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": false, 26 | "libName": "navx_frc", 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "linuxathena", 30 | "linuxraspbian", 31 | "windowsx86-64" 32 | ] 33 | } 34 | ] 35 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/RelativeOrientation.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Rotation2d; 4 | import frc.robot.Constants.DriveConstants; 5 | import frc.robot.drive.Drivetrain; 6 | import frc.lib.Curve; 7 | import frc.lib.HelixJoysticks; 8 | import frc.lib.LinCurve; 9 | 10 | //import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 11 | 12 | public class RelativeOrientation extends JoystickDrive { 13 | 14 | double dt = 0.02; //20 ms 15 | double joyScale = DriveConstants.kMaxRotationalVelocity * dt; 16 | Curve joyMap = new LinCurve(0.0, joyScale, 0.4); 17 | HelixJoysticks controller; 18 | 19 | public RelativeOrientation(Drivetrain subsystem, HelixJoysticks controller) { 20 | super(subsystem, controller); 21 | this.controller = controller; 22 | } 23 | 24 | @Override 25 | public double getTheta() { 26 | double thetaRaw = controller.getRotation(); 27 | double thetaTreated = joyMap.calculateMappedVal(thetaRaw); 28 | Rotation2d deltaTheta = new Rotation2d(thetaTreated); 29 | drivetrain.rotateRelative(deltaTheta); 30 | return drivetrain.getThetaDot(); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "2020.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "mavenUrls": [], 7 | "jsonUrl": "", 8 | "javaDependencies": [ 9 | { 10 | "groupId": "edu.wpi.first.wpilibNewCommands", 11 | "artifactId": "wpilibNewCommands-java", 12 | "version": "wpilib" 13 | } 14 | ], 15 | "jniDependencies": [], 16 | "cppDependencies": [ 17 | { 18 | "groupId": "edu.wpi.first.wpilibNewCommands", 19 | "artifactId": "wpilibNewCommands-cpp", 20 | "version": "wpilib", 21 | "libName": "wpilibNewCommands", 22 | "headerClassifier": "headers", 23 | "sourcesClassifier": "sources", 24 | "sharedLibrary": true, 25 | "skipInvalidPlatforms": true, 26 | "binaryPlatforms": [ 27 | "linuxathena", 28 | "linuxraspbian", 29 | "linuxaarch64bionic", 30 | "windowsx86-64", 31 | "windowsx86", 32 | "linuxx86-64", 33 | "osxx86-64" 34 | ] 35 | } 36 | ] 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/ZeroHeading.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Translation2d; 6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 7 | import edu.wpi.first.wpilibj2.command.CommandBase; 8 | import frc.robot.drive.Drivetrain; 9 | 10 | public class ZeroHeading extends CommandBase { 11 | // The subsystem the command runs on 12 | private final Drivetrain drivetrain; 13 | 14 | public ZeroHeading(Drivetrain subsystem){ 15 | drivetrain = subsystem; 16 | addRequirements(drivetrain); 17 | } 18 | 19 | @Override 20 | public void initialize() { 21 | } 22 | 23 | @Override 24 | public boolean runsWhenDisabled() { 25 | return true; 26 | } 27 | 28 | @Override 29 | public void execute() { 30 | SmartDashboard.putString("Zero Heading", "Resetting Heading"); 31 | drivetrain.zeroHeading(); 32 | drivetrain.resetOdometry(new Pose2d(new Translation2d(0,0),new Rotation2d(0))); 33 | } 34 | 35 | @Override 36 | public boolean isFinished() { 37 | return true; 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/VisionAutoShooter.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import frc.robot.drive.Drivetrain; 4 | import frc.robot.intake.Intake; 5 | import frc.robot.shooter.Shooter; 6 | import frc.robot.vision.Limelight; 7 | 8 | public class VisionAutoShooter extends VisionShooter { 9 | Drivetrain drivetrain; 10 | Intake intake; 11 | 12 | public VisionAutoShooter(Shooter shooter, Limelight limelight, Drivetrain drivetrain, Intake intake) { 13 | super(shooter, limelight); 14 | this.drivetrain = drivetrain; 15 | this.intake = intake; 16 | } 17 | 18 | @Override 19 | public void execute() { 20 | super.execute(); 21 | 22 | // Stopped? 23 | boolean stopped = drivetrain.getTranlationalVelocity() < 0.1; 24 | 25 | // Have target & aimed 26 | boolean onTarget = limelight.hasTarget() && (limelight.getState().xOffset < 5.0); 27 | 28 | // Within range 29 | boolean inRange = (distance > 1.5) && (distance < 3.5); 30 | 31 | boolean upToSpeed = rpmDelta < 50; 32 | 33 | if (stopped && onTarget && inRange && upToSpeed) { 34 | shooter.startTrigger(); 35 | intake.setPushCargo(true); 36 | } 37 | } 38 | } -------------------------------------------------------------------------------- /src/main/python/drive/diff_drive.py: -------------------------------------------------------------------------------- 1 | from math import hypot 2 | from casadi import * 3 | 4 | class swerve_drive: 5 | def __init__(self, wheelbase, mass, moi, length, width): 6 | self.wheelbase = wheelbase 7 | self.mass = mass 8 | self.moi = moi 9 | self.length = length 10 | self.width = width 11 | 12 | def add_kinematics_constraint(self, solver, theta, vx, vy, omega, ax, ay, alpha, N, f, v): 13 | for k in range(N): 14 | xc = cos(theta) 15 | yc = sin(theta) 16 | 17 | for module in modules: 18 | m_vx = vx[k] + module[0] * omega[k] 19 | m_vy = vy[k] + module[1] * omega[k] 20 | solver.subject_to(m_vx * m_vx + m_vy * m_vy < v*v) 21 | 22 | Fx = solver.variable(4) 23 | Fy = solver.variable(4) 24 | 25 | T = [] 26 | for j in range(4): 27 | T.append(modules[j][1] * Fx[j] - modules[j][0] * Fy[j]) 28 | solver.subject_to(Fx[j] * Fx[j] + Fy[j] * Fy[j] < f*f) 29 | 30 | solver.subject_to(ax[k] * self.mass == Fx[0] + Fx[1] + Fx[2] + Fx[3]) 31 | solver.subject_to(ay[k] * self.mass == Fy[0] + Fy[1] + Fy[2] + Fy[3]) 32 | solver.subject_to(alpha[k] == sum(T)) -------------------------------------------------------------------------------- /src/main/java/frc/lib/util/InterpolatingPoseMap.java: -------------------------------------------------------------------------------- 1 | package frc.lib.util; 2 | 3 | import java.util.TreeMap; 4 | 5 | import edu.wpi.first.math.geometry.Pose2d; 6 | 7 | public class InterpolatingPoseMap { 8 | private TreeMap history; 9 | private int mapSize; 10 | 11 | public InterpolatingPoseMap(int mapSize) { 12 | this.mapSize = mapSize; 13 | history = new TreeMap(); 14 | } 15 | 16 | public void addPose(Pose2d pose, double timestamp) { 17 | history.put(timestamp, pose); 18 | if (history.size() > mapSize) { 19 | history.remove(history.firstKey()); 20 | } 21 | } 22 | 23 | public Pose2d getLatestPose() { 24 | return history.lastEntry().getValue(); 25 | } 26 | 27 | public Pose2d getPose(double timestamp) { 28 | if (timestamp <= history.firstKey()) { 29 | return history.firstEntry().getValue(); 30 | } 31 | if (timestamp >= history.lastKey()) { 32 | return history.lastEntry().getValue(); 33 | } 34 | double low = history.floorKey(timestamp); 35 | double high = history.ceilingKey(timestamp); 36 | return history.get(low).interpolate(history.get(high), (timestamp - low) / (high - low)); 37 | } 38 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/FillLEDsCommand.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import edu.wpi.first.wpilibj.AddressableLEDBuffer; 4 | import edu.wpi.first.wpilibj.util.Color; 5 | import edu.wpi.first.wpilibj2.command.CommandBase; 6 | import frc.robot.status.Status; 7 | 8 | /** 9 | * Fll the LED strip with the color from the first 10 | * element in the LED Buffer. 11 | * 12 | * The usage for this is that pushing a shooter button sets the LED strip 13 | * with 1/2 a color and 1/2 off. When the wheel spins up to close to the 14 | * target speed, the strip should be completely filled with the color. 15 | * 16 | */ 17 | public class FillLEDsCommand extends CommandBase { 18 | private Status status; 19 | 20 | public FillLEDsCommand() { 21 | this.status = Status.getInstance(); 22 | addRequirements(status); 23 | } 24 | 25 | @Override 26 | public void initialize() { 27 | // Find the coloor of the first LED 28 | AddressableLEDBuffer buffer = status.getLedData(); 29 | Color color = buffer.getLED(0); 30 | // Set the color of the whole buffer 31 | status.setColor(color); 32 | } 33 | 34 | @Override 35 | public boolean isFinished() { 36 | return true; 37 | } 38 | 39 | } 40 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/SpinUpShooter.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.shooter.Shooter; 6 | 7 | public class SpinUpShooter extends CommandBase { 8 | Shooter shooter; 9 | int rpm; 10 | public SpinUpShooter(Shooter shooter, int rpm) { 11 | this.shooter = shooter; 12 | addRequirements(shooter); 13 | this.rpm = rpm; 14 | SmartDashboard.putNumber("Input Velocity", this.rpm); 15 | } 16 | 17 | // Called when the command is initially scheduled. 18 | @Override 19 | public void initialize() { 20 | 21 | } 22 | 23 | // Called every time the scheduler runs while the command is scheduled. 24 | @Override 25 | public void execute() { 26 | double velocity = SmartDashboard.getNumber("Input Velocity", 0); 27 | shooter.setShooterVelocity(velocity); 28 | SmartDashboard.putNumber("Shooter Velocity", shooter.getShooterVelocity()); 29 | } 30 | 31 | // Called once the command ends or is interrupted. 32 | @Override 33 | public void end(boolean interrupted) {} 34 | 35 | // Returns true when the command should end. 36 | @Override 37 | public boolean isFinished() { 38 | return false; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/XBoxButtonCommand.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import static com.team2363.utilities.ControllerMap.*; 4 | import edu.wpi.first.wpilibj.util.Color; 5 | import edu.wpi.first.wpilibj2.command.CommandBase; 6 | import frc.robot.status.Status; 7 | 8 | public class XBoxButtonCommand extends CommandBase{ 9 | private Status status; 10 | private Color color; 11 | private int brightness = 200; 12 | private int percent = 50; 13 | 14 | public XBoxButtonCommand(int button) { 15 | status = Status.getInstance(); 16 | addRequirements(status); 17 | 18 | switch (button) { 19 | case X_BOX_A: 20 | this.color = Color.kGreen; 21 | break; 22 | 23 | case X_BOX_B: 24 | this.color = Color.kRed; 25 | break; 26 | 27 | case X_BOX_X: 28 | this.color = Color.kBlue; 29 | break; 30 | 31 | case X_BOX_Y: 32 | this.color = Color.kYellow; 33 | break; 34 | 35 | default: 36 | this.color=Color.kMediumOrchid; 37 | break; 38 | } 39 | } 40 | 41 | @Override 42 | public void initialize() { 43 | status.setColor(color, brightness, percent); 44 | } 45 | 46 | @Override 47 | public boolean isFinished() { 48 | return true; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /vendordeps/photonlib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "photonlib.json", 3 | "name": "photonlib", 4 | "version": "v2022.1.4", 5 | "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", 6 | "mavenUrls": [ 7 | "https://maven.photonvision.org/repository/internal", 8 | "https://maven.photonvision.org/repository/snapshots" 9 | ], 10 | "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", 11 | "jniDependencies": [], 12 | "cppDependencies": [ 13 | { 14 | "groupId": "org.photonvision", 15 | "artifactId": "PhotonLib-cpp", 16 | "version": "v2022.1.4", 17 | "libName": "Photon", 18 | "headerClassifier": "headers", 19 | "sharedLibrary": true, 20 | "skipInvalidPlatforms": true, 21 | "binaryPlatforms": [ 22 | "windowsx86-64", 23 | "linuxathena", 24 | "linuxx86-64", 25 | "osxx86-64" 26 | ] 27 | } 28 | ], 29 | "javaDependencies": [ 30 | { 31 | "groupId": "org.photonvision", 32 | "artifactId": "PhotonLib-java", 33 | "version": "v2022.1.4" 34 | }, 35 | { 36 | "groupId": "org.photonvision", 37 | "artifactId": "PhotonTargeting-java", 38 | "version": "v2022.1.4" 39 | } 40 | ] 41 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/SetColor.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import edu.wpi.first.wpilibj.util.Color; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import frc.robot.status.Status; 6 | 7 | public class SetColor extends CommandBase{ 8 | private Status status; 9 | private Color color; 10 | private int brightness = 255; 11 | private int percent = 100; 12 | 13 | public SetColor(Status status, Color color) { 14 | this(status, color, 255); 15 | } 16 | 17 | public SetColor(Status status, Color color, int brightness) { 18 | this(status, color, brightness, 100); 19 | } 20 | 21 | /** 22 | * @param status - the Status object 23 | * @param color - the color to set 24 | * @param brightness - 0-255 25 | * @param percent - 0-100, percent of the buffer to fill, the rest will be set to black 26 | */ 27 | public SetColor(Status status, Color color, int brightness, int percent) { 28 | this.status = status; 29 | addRequirements(status); 30 | 31 | this.color = color; 32 | this.brightness = brightness; 33 | this.percent = percent; 34 | } 35 | 36 | 37 | @Override 38 | public void initialize() { 39 | status.setColor(color, brightness, percent); 40 | } 41 | 42 | @Override 43 | public boolean runsWhenDisabled() { 44 | return true; 45 | } 46 | 47 | @Override 48 | public boolean isFinished() { 49 | return true; 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/ActionCommand.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import frc.robot.status.actions.Action; 5 | import frc.robot.status.Status; 6 | 7 | public class ActionCommand extends CommandBase { 8 | 9 | private Status status = null; 10 | private Action action = null; 11 | 12 | // Default constructor. 13 | public ActionCommand(Action action) { 14 | this.status = Status.getInstance(); 15 | addRequirements(status); 16 | this.action = action; 17 | } 18 | 19 | 20 | // Called just before this Command runs the first time 21 | @Override 22 | public void initialize() { 23 | // HelixEvents.getInstance().addEvent("STATUS", "Starting ActionCommand"); 24 | 25 | // This will set our action to run on the subsystem. 26 | // This action will run in the dedicated thread to output the rainbow pattern. 27 | status.setAction(action); 28 | } 29 | 30 | // Called repeatedly when this Command is scheduled to run 31 | @Override 32 | public void execute() { 33 | // Nothing to do, extending is handled when initialized. 34 | } 35 | 36 | // Called once after isFinished returns true 37 | @Override 38 | public void end(boolean interrupted) { 39 | // HelixEvents.getInstance().addEvent("STATUS", "Ending ActionCommand"); 40 | } 41 | 42 | // Make this return true when this Command no longer needs to run execute() 43 | @Override 44 | public boolean isFinished() { 45 | return true; 46 | } 47 | 48 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/JoystickDrive.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import frc.robot.drive.Drivetrain; 4 | 5 | // import com.team2363.utilities.ControllerMap; 6 | 7 | // import edu.wpi.first.wpilibj.Joystick; 8 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 9 | import frc.lib.Curve; 10 | import frc.lib.HelixJoysticks; 11 | import frc.lib.LinCurve; 12 | 13 | public class JoystickDrive extends Drive { 14 | 15 | Curve xyJoyMap = new LinCurve(0.0, 1.0, 0.2); 16 | Curve thetaJoyMap = new LinCurve(0.0, 1.0, 0.2); 17 | 18 | HelixJoysticks m_controller; 19 | 20 | public JoystickDrive(Drivetrain subsystem, HelixJoysticks joysticks){ 21 | super(subsystem); 22 | this.m_controller = joysticks; 23 | } 24 | 25 | @Override 26 | public double getX() { 27 | double xRaw = m_controller.getX(); 28 | // SmartDashboard.putNumber("JoystickX", xyJoyMap.calculateMappedVal(xRaw)); 29 | return xyJoyMap.calculateMappedVal(xRaw); 30 | } 31 | 32 | @Override 33 | public double getY() { 34 | double yRaw = m_controller.getY(); 35 | // SmartDashboard.putNumber("JoystickY", xyJoyMap.calculateMappedVal(yRaw)); 36 | return xyJoyMap.calculateMappedVal(yRaw); 37 | } 38 | 39 | @Override 40 | public double getTheta() { 41 | double thetaRaw = m_controller.getRotation(); 42 | // SmartDashboard.putNumber("JoystickTheta", xyJoyMap.calculateMappedVal(thetaRaw)); 43 | return thetaJoyMap.calculateMappedVal(thetaRaw); 44 | } 45 | 46 | @Override 47 | public boolean getFieldRelative() { 48 | return true; 49 | } 50 | } -------------------------------------------------------------------------------- /src/main/java/frc/lib/LinCurve.java: -------------------------------------------------------------------------------- 1 | package frc.lib; 2 | /** 3 | * This class maps the value of a stick input to an linear curve. 4 | * It is a subclass of Curve. 5 | * 6 | * @author Justin Babilino 7 | * @version 0.0.3 8 | */ 9 | public class LinCurve extends Curve { 10 | /** 11 | * Constructs an Linear Curve object which can 12 | * be used to map a stick input proportionally. 13 | * Initialized with values provided. 14 | * 15 | * @param offset value used to offset the final curve 16 | * @param scalar value used to scale the value before offset 17 | * @param deadzone value for the width of the deadband in the center of the curve 18 | */ 19 | public LinCurve(double offset, double scalar, double deadzone) { 20 | setOffset(offset); 21 | setScalar(scalar); 22 | setDeadzone(deadzone); 23 | } 24 | 25 | /** 26 | * Constructs an Linear Curve object which can 27 | * be used to map a stick input proportionally. 28 | * Initialized with default values: 29 | * 30 | * offset = 0.0; 31 | * scalar = 1.0; 32 | * deadzone = 0.0; 33 | * 34 | */ 35 | public LinCurve() { 36 | setOffset(0.0); 37 | setScalar(1.0); 38 | setDeadzone(0.0); 39 | } 40 | 41 | /** 42 | * @param input value to be mapped 43 | */ 44 | @Override public double calculateMappedVal(double input) { 45 | double val = calculateOffset(calculateScalar(calculateDeadzone(input))); 46 | if (val > 1.0) { 47 | val = 1.0; 48 | } else if (val < -1.0) { 49 | val = -1.0; 50 | } 51 | return val; 52 | } 53 | } -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/PIDController.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | public class PIDController { 4 | private double kP, kD, kI; 5 | private double derivative, integral, lastError; 6 | private double reference; 7 | private double inputRange; 8 | private boolean continous; 9 | 10 | public PIDController(double kP, double kI, double kD) { 11 | this.kP = kP; 12 | this.kI = kI; 13 | this.kD = kD; 14 | integral = 0; 15 | lastError = 0; 16 | inputRange = Double.POSITIVE_INFINITY; 17 | continous = false; 18 | } 19 | 20 | public double calculate(double state, double dt) { 21 | double error = (reference - state) % inputRange; 22 | 23 | if (Math.abs(error) > inputRange / 2) { 24 | if (error > 0) { 25 | error -= inputRange; 26 | } else { 27 | error += inputRange; 28 | } 29 | } 30 | 31 | if (dt > 0) { 32 | derivative = (error - lastError) / dt; 33 | integral += error * dt; 34 | } 35 | 36 | lastError = error; 37 | 38 | return kP * error + kI * integral + kD * derivative; 39 | } 40 | 41 | public void setP(double P) { 42 | kP = P; 43 | } 44 | 45 | public void setD(double D) { 46 | kD = D; 47 | } 48 | 49 | public void setReference(double reference) { 50 | this.reference = reference; 51 | } 52 | 53 | public void setInputRange(double inputRange) { 54 | this.inputRange = inputRange; 55 | } 56 | 57 | public void setContinous(boolean continous) { 58 | this.continous = continous; 59 | } 60 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/Drive.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | import frc.robot.Constants; 7 | import frc.robot.Constants.DriveConstants; 8 | import frc.robot.drive.Drivetrain; 9 | 10 | public abstract class Drive extends CommandBase { 11 | 12 | private double xDot; 13 | private double yDot; 14 | private double thetaDot; 15 | private boolean fieldRelative; 16 | private ChassisSpeeds chassisSpeeds, chassisPercent; 17 | 18 | // The subsystem the command runs on 19 | public final Drivetrain drivetrain; 20 | 21 | public Drive(Drivetrain subsystem){ 22 | drivetrain = subsystem; 23 | addRequirements(drivetrain); 24 | } 25 | 26 | @Override 27 | public void initialize() { 28 | } 29 | 30 | @Override 31 | public void execute() { 32 | xDot = getX() * DriveConstants.kMaxTranslationalVelocity; 33 | yDot = getY() * DriveConstants.kMaxTranslationalVelocity; 34 | thetaDot = getTheta() * DriveConstants.kMaxRotationalVelocity; 35 | fieldRelative = getFieldRelative(); 36 | 37 | chassisSpeeds = fieldRelative 38 | ? ChassisSpeeds.fromFieldRelativeSpeeds(xDot, yDot, thetaDot, drivetrain.getHeading()) 39 | : new ChassisSpeeds(xDot, yDot, thetaDot); 40 | 41 | drivetrain.drive(chassisSpeeds, true); 42 | } 43 | 44 | abstract public double getX(); 45 | abstract public double getY(); 46 | abstract public double getTheta(); 47 | abstract public boolean getFieldRelative(); 48 | } -------------------------------------------------------------------------------- /src/main/python/trajectory_io.py: -------------------------------------------------------------------------------- 1 | import json 2 | from math import * 3 | import json 4 | 5 | def export_trajectory(x, y, theta, vx, vy, omega, dts, N_per_segment, name): 6 | ts = [0] 7 | for dt in dts: 8 | for k in range(N_per_segment): 9 | ts.append(dt + ts[-1]) 10 | xs, ys, thetas = [], [], [] 11 | new_dt = 0.02 12 | time = 0 13 | index = 0 14 | for k in range(int(ts[-1] / 0.02)): 15 | while ts[index + 1] < k * 0.02: 16 | index += 1 17 | percent = (k * 0.02 - ts[index]) / (ts[index + 1] - ts[index]) 18 | xs.append(round((x[index + 1] - x[index]) * percent + x[index], 4)) 19 | ys.append(round((y[index + 1] - y[index]) * percent + y[index], 4)) 20 | thetas.append(round((theta[index + 1] - theta[index]) * percent + theta[index], 4)) 21 | xs.append(x[-1]) 22 | ys.append(y[-1]) 23 | thetas.append(theta[-1]) 24 | 25 | f = open("src/main/java/frc/paths/" + name + ".java", "w") 26 | f.write("package frc.paths;\n") 27 | f.write("\n") 28 | f.write("import frc.lib.control.SwerveTrajectory;\n") 29 | f.write("\n") 30 | f.write("public class " + name + " extends Path {\n") 31 | f.write(" private final static double[][] points = {\n") 32 | for j in range(len(x)): 33 | f.write(" {"+str(round(ts[j],4))+","+str(round(x[j],4))+","+str(round(y[j],4))+","+str(round(theta[j],4))+","+str(round(vx[j],4))+","+str(round(vy[j],4))+","+str(round(omega[j],4))+"},\n") 34 | f.write(" };\n") 35 | f.write(" public SwerveTrajectory getPath() {\n") 36 | f.write(" return new SwerveTrajectory(points);\n") 37 | f.write(" }\n") 38 | f.write("}\n") 39 | f.close() 40 | 41 | return xs, ys, theta 42 | 43 | def import_path(file): 44 | return json.load(open("src/main/python/paths/" + file + ".json")) 45 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/TestDrive.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Rotation2d; 4 | import edu.wpi.first.math.kinematics.SwerveModuleState; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | import edu.wpi.first.wpilibj2.command.CommandBase; 7 | import frc.robot.drive.Drivetrain; 8 | 9 | public class TestDrive extends CommandBase { 10 | Drivetrain drive; 11 | 12 | public TestDrive(Drivetrain drive) { 13 | addRequirements(drive); 14 | this.drive = drive; 15 | } 16 | 17 | // Called when the command is initially scheduled. 18 | @Override 19 | public void initialize() { 20 | SmartDashboard.putNumber("Velocity 0", 0); 21 | SmartDashboard.putNumber("Velocity 1", 0); 22 | SmartDashboard.putNumber("Velocity 2", 0); 23 | SmartDashboard.putNumber("Velocity 3", 0); 24 | } 25 | 26 | // Called every time the scheduler runs while the command is scheduled. 27 | @Override 28 | public void execute() { 29 | double v0 = SmartDashboard.getNumber("Velocity 0", 0); 30 | double v1 = SmartDashboard.getNumber("Velocity 1", 0); 31 | double v2 = SmartDashboard.getNumber("Velocity 2", 0); 32 | double v3 = SmartDashboard.getNumber("Velocity 3", 0); 33 | SwerveModuleState s0 = new SwerveModuleState(v0, new Rotation2d(0)); 34 | SwerveModuleState s1 = new SwerveModuleState(v1, new Rotation2d(0)); 35 | SwerveModuleState s2 = new SwerveModuleState(v2, new Rotation2d(0)); 36 | SwerveModuleState s3 = new SwerveModuleState(v3, new Rotation2d(0)); 37 | SwerveModuleState[] states = new SwerveModuleState[]{s0, s1, s2, s3}; 38 | drive.setModuleStates(states); 39 | } 40 | 41 | // Called once the command ends or is interrupted. 42 | @Override 43 | public void end(boolean interrupted) {} 44 | 45 | // Returns true when the command should end. 46 | @Override 47 | public boolean isFinished() { 48 | return false; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/ShootAndDriveForward.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | 5 | import javax.swing.GroupLayout.ParallelGroup; 6 | 7 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 8 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 9 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 10 | import frc.paths.OnePointEightMetersForward; 11 | import frc.robot.drive.Drivetrain; 12 | import frc.robot.drive.commands.TrajectoryFollower; 13 | import frc.robot.intake.Intake; 14 | import frc.robot.intake.commands.RetractIntake; 15 | import frc.robot.shooter.Shooter; 16 | import frc.robot.shooter.commands.FlywheelController; 17 | import frc.robot.shooter.commands.PullTrigger; 18 | import frc.robot.shooter.commands.ResetHood; 19 | import frc.robot.shooter.commands.SetShooterState; 20 | import frc.robot.shooter.commands.StopShooter; 21 | import frc.robot.shooter.commands.StopTrigger; 22 | import frc.robot.Constants.ShooterConstants; 23 | 24 | public class ShootAndDriveForward extends SequentialCommandGroup{ 25 | 26 | public ShootAndDriveForward(Drivetrain drive, Intake intake, Shooter shooter) { 27 | addCommands( 28 | new ParallelDeadlineGroup( 29 | new SequentialCommandGroup( 30 | new WaitCommand(1.0), // Give shooter time to spin up & hood to move 31 | new PullTrigger(shooter, intake), 32 | new WaitCommand(1.25) 33 | ), 34 | new FlywheelController(shooter, 1700, 79) 35 | ), 36 | new StopTrigger(shooter, intake), 37 | new StopShooter(shooter), // Stop shooter & reset hood. 38 | new TrajectoryFollower(drive, new OnePointEightMetersForward()), 39 | new TrajectoryFollower(drive, new OnePointEightMetersForward()), 40 | new ResetHood(shooter) 41 | ); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /simgui-ds.json: -------------------------------------------------------------------------------- 1 | { 2 | "keyboardJoysticks": [ 3 | { 4 | "axisConfig": [ 5 | { 6 | "decKey": 65, 7 | "incKey": 68 8 | }, 9 | { 10 | "decKey": 87, 11 | "incKey": 83 12 | }, 13 | { 14 | "decKey": 69, 15 | "decayRate": 0.0, 16 | "incKey": 82, 17 | "keyRate": 0.009999999776482582 18 | } 19 | ], 20 | "axisCount": 3, 21 | "buttonCount": 4, 22 | "buttonKeys": [ 23 | 90, 24 | 88, 25 | 67, 26 | 86 27 | ], 28 | "povConfig": [ 29 | { 30 | "key0": 328, 31 | "key135": 323, 32 | "key180": 322, 33 | "key225": 321, 34 | "key270": 324, 35 | "key315": 327, 36 | "key45": 329, 37 | "key90": 326 38 | } 39 | ], 40 | "povCount": 1 41 | }, 42 | { 43 | "axisConfig": [ 44 | { 45 | "decKey": 74, 46 | "incKey": 76 47 | }, 48 | { 49 | "decKey": 73, 50 | "incKey": 75 51 | } 52 | ], 53 | "axisCount": 2, 54 | "buttonCount": 4, 55 | "buttonKeys": [ 56 | 77, 57 | 44, 58 | 46, 59 | 47 60 | ], 61 | "povCount": 0 62 | }, 63 | { 64 | "axisConfig": [ 65 | { 66 | "decKey": 263, 67 | "incKey": 262 68 | }, 69 | { 70 | "decKey": 265, 71 | "incKey": 264 72 | } 73 | ], 74 | "axisCount": 2, 75 | "buttonCount": 6, 76 | "buttonKeys": [ 77 | 260, 78 | 268, 79 | 266, 80 | 261, 81 | 269, 82 | 267 83 | ], 84 | "povCount": 0 85 | }, 86 | { 87 | "axisCount": 0, 88 | "buttonCount": 0, 89 | "povCount": 0 90 | } 91 | ], 92 | "robotJoysticks": [ 93 | { 94 | "guid": "030000005e040000fd02000003090000", 95 | "useGamepad": true 96 | } 97 | ] 98 | } 99 | -------------------------------------------------------------------------------- /src/main/python/drive/swerve_drive.py: -------------------------------------------------------------------------------- 1 | from math import hypot 2 | from casadi import * 3 | 4 | class swerve_drive: 5 | def __init__(self, wheelbase_x, wheelbase_y, length, width, mass, moi, omega_max, tau_max, wheel_radius): 6 | self.wheelbase_x = wheelbase_x 7 | self.wheelbase_y = wheelbase_y 8 | self.length = length 9 | self.width = width 10 | self.mass = mass 11 | self.moi = moi 12 | self.omega_max = omega_max 13 | self.tau_max = tau_max 14 | self.wheel_radius = wheel_radius 15 | self.force_max = tau_max / wheel_radius 16 | 17 | def solve_module_positions(self, k, theta): 18 | modules, thetas = [], [] 19 | d = hypot(self.wheelbase_x, self.wheelbase_y) 20 | for a in [1, -1]: 21 | for b in [1, -1]: 22 | thetas.append(atan2(self.wheelbase_y*a, self.wheelbase_x*b)) 23 | for module_theta in thetas: 24 | modules.append([d*cos(module_theta+theta[k]), d*sin(module_theta+theta[k])]) 25 | return modules 26 | 27 | def add_kinematics_constraint(self, solver, theta, vx, vy, omega, ax, ay, alpha, N): 28 | for k in range(N): 29 | modules = self.solve_module_positions(k, theta) 30 | 31 | max_v = self.omega_max * self.wheel_radius 32 | for module in modules: 33 | m_vx = vx[k] + module[0] * omega[k] 34 | m_vy = vy[k] + module[1] * omega[k] 35 | solver.subject_to(m_vx * m_vx + m_vy * m_vy < max_v * max_v) 36 | 37 | Fx = solver.variable(4) 38 | Fy = solver.variable(4) 39 | 40 | T = [] 41 | for j in range(4): 42 | T.append(modules[j][1] * Fx[j] - modules[j][0] * Fy[j]) 43 | solver.subject_to(Fx[j] * Fx[j] + Fy[j] * Fy[j] < self.force_max * self.force_max) 44 | 45 | solver.subject_to(ax[k] * self.mass == Fx[0] + Fx[1] + Fx[2] + Fx[3]) 46 | solver.subject_to(ay[k] * self.mass == Fy[0] + Fy[1] + Fy[2] + Fy[3]) 47 | solver.subject_to(alpha[k] * self.moi == sum(T)) -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/groups/LEDDemoCG.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.groups; 2 | 3 | import edu.wpi.first.wpilibj.Filesystem; 4 | import edu.wpi.first.wpilibj.util.Color; 5 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 6 | import edu.wpi.first.wpilibj2.command.WaitCommand; 7 | import frc.robot.status.Status; 8 | import frc.robot.status.actions.ImageAction; 9 | import frc.robot.status.actions.LedAction; 10 | import frc.robot.status.commands.ActionCommand; 11 | import frc.robot.status.commands.SetColor; 12 | 13 | import java.io.File; 14 | 15 | public class LEDDemoCG extends SequentialCommandGroup { 16 | 17 | public LEDDemoCG() { 18 | // File deployDir = Filesystem.getDeployDirectory(); 19 | // String pathPrefix = deployDir.getAbsolutePath() + "/images/"; 20 | 21 | ImageAction ia = new ImageAction("THfade.png").oscillate(); 22 | // addCommands(new ActionCommand(ia)); 23 | // addCommands(new WaitCommand(10)); 24 | 25 | addCommands(new ActionCommand(new ImageAction("fade.png",0.02).oscillate().brightness(0.7))); 26 | addCommands(new WaitCommand(10)); 27 | 28 | ia = new ImageAction("noise.png", 0.25); 29 | addCommands(new ActionCommand(ia)); 30 | addCommands(new WaitCommand(10)); 31 | 32 | ia = new ImageAction("noisy-pulse.png"); 33 | addCommands(new ActionCommand(ia)); 34 | addCommands(new WaitCommand(10)); 35 | 36 | ia = new ImageAction("pulse.png").oscillate(); 37 | addCommands(new ActionCommand(ia)); 38 | addCommands(new WaitCommand(10)); 39 | 40 | ia = new ImageAction("pulse-down.png").oscillate(); 41 | addCommands(new ActionCommand(ia)); 42 | addCommands(new WaitCommand(10)); 43 | 44 | ia = new ImageAction("stripes.png").oscillate(); 45 | addCommands(new ActionCommand(ia)); 46 | addCommands(new WaitCommand(10)); 47 | 48 | ia = new ImageAction("fade.png").oscillate(); 49 | addCommands(new ActionCommand(ia)); 50 | addCommands(new WaitCommand(10)); 51 | 52 | addCommands(new SetColor(Status.getInstance(), Color.kBlack)); 53 | } 54 | 55 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/vision/Limelight.java: -------------------------------------------------------------------------------- 1 | package frc.robot.vision; 2 | 3 | import edu.wpi.first.networktables.EntryListenerFlags; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | 7 | import static edu.wpi.first.networktables.NetworkTableInstance.getDefault; 8 | 9 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 10 | 11 | public class Limelight extends SubsystemBase { 12 | private volatile VisionState state = new VisionState(0,0,0); 13 | 14 | double h = 2.1; 15 | double theta = 45; 16 | 17 | public Limelight() { 18 | getDefault().getTable("limelight").getEntry("tx").addListener(event -> { 19 | if (getDefault().getTable("limelight").getEntry("tv").getDouble(0) == 1) { 20 | double xOffset = getDefault().getTable("limelight").getEntry("tx").getDouble(0); 21 | double yOffset = getDefault().getTable("limelight").getEntry("ty").getDouble(0); 22 | double distance = h/(Math.tan(Math.toRadians(yOffset+theta))*Math.cos(Math.toRadians(xOffset))); 23 | SmartDashboard.putNumber("Distance", distance); 24 | double latency = getDefault().getTable("limelight").getEntry("tl").getDouble(0) / 1000.0 + 0.011; 25 | state = new VisionState(xOffset, distance, Timer.getFPGATimestamp() - latency); 26 | } 27 | }, EntryListenerFlags.kUpdate); 28 | } 29 | 30 | public VisionState getState() { 31 | return state; 32 | } 33 | 34 | public boolean hasTarget() { 35 | return getDefault().getTable("limelight").getEntry("tv").getDouble(0) == 1.0; 36 | } 37 | 38 | public void turnOffLEDs() { 39 | getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); 40 | } 41 | 42 | public void turnOnLEDs() { 43 | getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); 44 | } 45 | 46 | public static class VisionState { 47 | public final double xOffset; 48 | public final double distance; 49 | public final double timestamp; 50 | 51 | public VisionState(double xOffset, double distance, double timestamp) { 52 | this.xOffset = xOffset; 53 | this.distance = distance; 54 | this.timestamp = timestamp; 55 | } 56 | } 57 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/DIOSwitchStatus.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import frc.robot.status.Status; 4 | import edu.wpi.first.wpilibj.AddressableLEDBuffer; 5 | import edu.wpi.first.wpilibj.util.Color; 6 | import edu.wpi.first.wpilibj2.command.CommandBase; 7 | 8 | public class DIOSwitchStatus extends CommandBase { 9 | private Status status; 10 | int dioState = -1; 11 | int ledOffset = 10; 12 | AddressableLEDBuffer buffer = new AddressableLEDBuffer(Status.ADDRESSABLE_LED_COUNT); 13 | 14 | public DIOSwitchStatus(int dioState) { 15 | this.status = Status.getInstance(); 16 | this.dioState = dioState; 17 | addRequirements(status); 18 | } 19 | 20 | @Override 21 | public boolean runsWhenDisabled() { 22 | return true; 23 | } 24 | 25 | @Override 26 | public void initialize() { 27 | // Set the buffer to black to start 28 | for (var i = 0; i < buffer.getLength(); i++) { 29 | buffer.setLED(i, Color.kBlack); 30 | } 31 | } 32 | 33 | @Override 34 | public void execute() { 35 | if (dioState < 0) { 36 | buffer.setLED(0, Color.kDarkRed); 37 | } else { 38 | if (dioState >= 0) { 39 | buffer.setLED(ledOffset+ 0, Color.kDarkRed); 40 | buffer.setLED(ledOffset+ 1, Color.kDarkRed); 41 | } 42 | if (dioState >= 1) { 43 | buffer.setLED(ledOffset+ 2, Color.kOlive); 44 | buffer.setLED(ledOffset+ 3, Color.kOlive); 45 | } 46 | if (dioState >= 2) { 47 | buffer.setLED(ledOffset+ 4, Color.kDarkGreen); 48 | buffer.setLED(ledOffset+ 5, Color.kDarkGreen); 49 | } 50 | if (dioState >= 3) { 51 | buffer.setLED(ledOffset+ 6, Color.kDarkBlue); 52 | buffer.setLED(ledOffset+ 7, Color.kDarkBlue); 53 | } 54 | if (dioState >= 4) { 55 | buffer.setLED(ledOffset+ 8, Color.kDarkGray); 56 | buffer.setLED(ledOffset+ 9, Color.kDarkGray); 57 | } 58 | } 59 | status.setLedData(buffer); 60 | } 61 | 62 | @Override 63 | public boolean isFinished() { 64 | return true; 65 | } 66 | 67 | } 68 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/ChaseAction.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | public class ChaseAction extends LedAction { 4 | 5 | // State 6 | private int leadingIndex = 0; 7 | private double chaseFactor = 0.10; 8 | 9 | private int red = 0; 10 | private int green = 0; 11 | private int blue = 0; 12 | private int brightness = 0; 13 | 14 | // Default will run a rainbow pattern. 15 | public ChaseAction() { 16 | super(); 17 | 18 | red = 255; 19 | brightness = 100; 20 | 21 | // Run forever, 10ms 22 | intervalCount = -1; 23 | intervalTime = 0.010; 24 | } 25 | 26 | public ChaseAction(int red, int green, int blue, int brightness) { 27 | super(); 28 | 29 | this.red = red; 30 | this.green = green; 31 | this.blue = blue; 32 | this.brightness = brightness; 33 | 34 | // Run forever, 50ms 35 | intervalCount = -1; 36 | intervalTime = 0.050; 37 | } 38 | 39 | protected void updateBuffer() { 40 | 41 | int r = (int) (red * (brightness / 255.0)); 42 | int g = (int) (green * (brightness / 255.0)); 43 | int b = (int) (blue * (brightness / 255.0)); 44 | 45 | int chaseCount = (int) (buffer.getLength() * chaseFactor); 46 | 47 | for (var i = 0; i < buffer.getLength(); i++) { 48 | 49 | if (i == leadingIndex) { 50 | // The leading pixel. 51 | buffer.setRGB(i, r, g, b); 52 | } else { 53 | 54 | int d = 0; 55 | 56 | if ((i < leadingIndex) && (i > leadingIndex - chaseCount)) { 57 | d = leadingIndex - i; 58 | } 59 | 60 | if (d != 0) { 61 | int cr = (int) (r * (1 - d * chaseFactor)); 62 | int cg = (int) (g * (1 - d * chaseFactor)); 63 | int cb = (int) (b * (1 - d * chaseFactor)); 64 | 65 | buffer.setRGB(i, cr, cg, cb); 66 | } else { 67 | buffer.setRGB(i, 245, 0, 255); 68 | } 69 | } 70 | } 71 | 72 | ++leadingIndex; 73 | if (leadingIndex == buffer.getLength()) { 74 | leadingIndex = 0; 75 | } 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/TurnToAngle.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import frc.lib.HelixJoysticks; 4 | import frc.lib.control.PIDController; 5 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 6 | import edu.wpi.first.wpilibj2.command.CommandBase; 7 | import frc.robot.Constants.DriveConstants; 8 | import frc.robot.drive.Drivetrain; 9 | import frc.robot.vision.Limelight; 10 | import frc.robot.vision.Limelight.VisionState; 11 | 12 | public class TurnToAngle extends CommandBase { 13 | private Drivetrain drive; 14 | private Limelight limelight; 15 | private PIDController thetaController; 16 | private HelixJoysticks joysticks; 17 | private boolean sawTarget; 18 | 19 | public TurnToAngle(Drivetrain drive, Limelight limelight, HelixJoysticks joysticks) { 20 | addRequirements(drive); 21 | this.drive = drive; 22 | this.limelight = limelight; 23 | this.joysticks = joysticks; 24 | } 25 | 26 | @Override 27 | public void initialize() { 28 | thetaController = new PIDController(0.16, 0, 0.0); 29 | thetaController.setContinous(true); 30 | thetaController.setInputRange(360); 31 | limelight.turnOnLEDs(); 32 | // sawTarget = limelight.hasTarget(); 33 | } 34 | 35 | @Override 36 | public void execute() { 37 | // sawTarget = limelight.hasTarget(); 38 | // if (sawTarget) { 39 | VisionState state = limelight.getState(); 40 | 41 | thetaController.setReference(drive.getPose(state.timestamp).getRotation().getDegrees() - state.xOffset); 42 | 43 | double omega = -thetaController.calculate(drive.getPose().getRotation().getDegrees(), 0.02); 44 | 45 | drive.openLoopDrive(ChassisSpeeds.fromFieldRelativeSpeeds( 46 | joysticks.getX() * DriveConstants.kMaxTranslationalVelocity, 47 | joysticks.getY() * DriveConstants.kMaxTranslationalVelocity, 48 | omega, 49 | drive.getHeading())); 50 | // } 51 | 52 | } 53 | 54 | @Override 55 | public void end(boolean interrupted) { 56 | drive.brake(); 57 | // limelight.turnOffLEDs(); 58 | } 59 | 60 | @Override 61 | public boolean isFinished() { 62 | return false; 63 | } 64 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/PowerUpAction.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | import edu.wpi.first.wpilibj.util.Color; 4 | import edu.wpi.first.wpilibj.util.Color8Bit; 5 | 6 | public class PowerUpAction extends LedAction { 7 | 8 | // State 9 | private int leadingIndex = 0; 10 | 11 | private int red = 0; 12 | private int green = 0; 13 | private int blue = 0; 14 | private int brightness = 0; 15 | 16 | // Default will run a red pattern. 17 | public PowerUpAction() { 18 | super(); 19 | 20 | red = 255; 21 | brightness = 100; 22 | 23 | // Run forever, 10ms 24 | intervalCount = -1; 25 | intervalTime = 0.010; 26 | } 27 | 28 | /** 29 | * PowerUp animation using a Color 30 | * 31 | * @param color 32 | * @param brightness 0.0 to 1.0 33 | */ 34 | public PowerUpAction(Color color, double brightness) { 35 | int intBrightness = 0; 36 | if (0.0 > brightness) { 37 | intBrightness = 0; 38 | } if (1.0 < brightness) { 39 | intBrightness = 255; 40 | } else { 41 | intBrightness = (int) (255 * brightness); 42 | } 43 | Color8Bit intColor = new Color8Bit(color); 44 | 45 | this.brightness = intBrightness; 46 | this.red = intColor.red; 47 | this.green = intColor.green; 48 | this.blue = intColor.blue; 49 | 50 | // Run forever, 10ms 51 | intervalCount = -1; 52 | intervalTime = 0.010; 53 | } 54 | 55 | public PowerUpAction(int red, int green, int blue, int brightness) { 56 | super(); 57 | 58 | this.red = red; 59 | this.green = green; 60 | this.blue = blue; 61 | this.brightness = brightness; 62 | 63 | // Run forever, 10ms 64 | intervalCount = -1; 65 | intervalTime = 0.010; 66 | } 67 | 68 | protected void updateBuffer() { 69 | 70 | int r = (int) (red * (brightness / 255.0)); 71 | int g = (int) (green * (brightness / 255.0)); 72 | int b = (int) (blue * (brightness / 255.0)); 73 | 74 | for (var i = 0; i < buffer.getLength(); i++) { 75 | if (i <= leadingIndex) { 76 | // The leading pixel. 77 | buffer.setRGB(i, r, g, b); 78 | } 79 | } 80 | ++leadingIndex; 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/MoveHoodJoystick.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import com.revrobotics.CANSparkMax; 4 | import com.team2363.utilities.ControllerMap; 5 | 6 | import edu.wpi.first.wpilibj.Joystick; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | import edu.wpi.first.wpilibj2.command.CommandBase; 9 | import frc.robot.Constants.ShooterConstants; 10 | import frc.robot.shooter.Shooter; 11 | 12 | /** 13 | * Raise the hood. 14 | * Hood moves at a fixed rate - to limit stress on components. 15 | * Hood hard stops based on current draw. 16 | * 17 | */ 18 | public class MoveHoodJoystick extends CommandBase { 19 | 20 | private Shooter shooter; 21 | private Joystick joystick; 22 | 23 | public MoveHoodJoystick(Shooter shooter, Joystick joystick) { 24 | this.joystick = joystick; 25 | this.shooter = shooter; 26 | addRequirements(shooter); 27 | // Use addRequirements() here to declare subsystem dependencies. 28 | } 29 | 30 | // Called when the command is initially scheduled. 31 | @Override 32 | public void initialize() { 33 | // Start the hood motor. 34 | // shooter.moveHood(direction); 35 | double joystickInput = Math.max(0, -joystick.getRawAxis(ControllerMap.PS4_LEFT_STICK_Y)); 36 | SmartDashboard.putNumber("Hood Position", shooter.getHoodAngle()); 37 | double target = ((ShooterConstants.kHoodMaxAngle - ShooterConstants.kHoodMinAngle) * joystickInput + ShooterConstants.kHoodMinAngle); 38 | shooter.setHoodPosition(target); 39 | } 40 | 41 | // Called every time the scheduler runs while the command is scheduled. 42 | @Override 43 | public void execute() { 44 | // Check if hard stop hit. Stop if so. 45 | // if ((Shooter.UP == direction) && (ShooterConstants.kHoodMaxAngle < shooter.getHoodAngle()) 46 | // || (Shooter.DOWN == direction) && (ShooterConstants.kHoodMinAngle > shooter.getHoodAngle())) { 47 | // end(true); 48 | // } 49 | 50 | } 51 | 52 | // Called once the command ends or is interrupted. 53 | @Override 54 | public void end(boolean interrupted) { 55 | // Stop the motor 56 | // shooter.stopHood(); 57 | } 58 | 59 | // Returns true when the command should end. 60 | @Override 61 | public boolean isFinished() { 62 | return true; 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /vendordeps/REVLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVLib.json", 3 | "name": "REVLib", 4 | "version": "2022.1.1", 5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 6 | "mavenUrls": [ 7 | "https://maven.revrobotics.com/" 8 | ], 9 | "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.revrobotics.frc", 13 | "artifactId": "REVLib-java", 14 | "version": "2022.1.1" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.revrobotics.frc", 20 | "artifactId": "REVLib-driver", 21 | "version": "2022.1.1", 22 | "skipInvalidPlatforms": true, 23 | "isJar": false, 24 | "validPlatforms": [ 25 | "windowsx86-64", 26 | "windowsx86", 27 | "linuxaarch64bionic", 28 | "linuxx86-64", 29 | "linuxathena", 30 | "linuxraspbian", 31 | "osxx86-64" 32 | ] 33 | } 34 | ], 35 | "cppDependencies": [ 36 | { 37 | "groupId": "com.revrobotics.frc", 38 | "artifactId": "REVLib-cpp", 39 | "version": "2022.1.1", 40 | "libName": "REVLib", 41 | "headerClassifier": "headers", 42 | "sharedLibrary": false, 43 | "skipInvalidPlatforms": true, 44 | "binaryPlatforms": [ 45 | "windowsx86-64", 46 | "windowsx86", 47 | "linuxaarch64bionic", 48 | "linuxx86-64", 49 | "linuxathena", 50 | "linuxraspbian", 51 | "osxx86-64" 52 | ] 53 | }, 54 | { 55 | "groupId": "com.revrobotics.frc", 56 | "artifactId": "REVLib-driver", 57 | "version": "2022.1.1", 58 | "libName": "REVLibDriver", 59 | "headerClassifier": "headers", 60 | "sharedLibrary": false, 61 | "skipInvalidPlatforms": true, 62 | "binaryPlatforms": [ 63 | "windowsx86-64", 64 | "windowsx86", 65 | "linuxaarch64bionic", 66 | "linuxx86-64", 67 | "linuxathena", 68 | "linuxraspbian", 69 | "osxx86-64" 70 | ] 71 | } 72 | ] 73 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/MoveHoodButton.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import com.revrobotics.CANSparkMax; 4 | import com.team2363.utilities.ControllerMap; 5 | 6 | import edu.wpi.first.wpilibj.Joystick; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | import edu.wpi.first.wpilibj2.command.CommandBase; 9 | import frc.robot.Constants.ShooterConstants; 10 | import frc.robot.shooter.Shooter; 11 | 12 | /** 13 | * Raise the hood. 14 | * Hood moves at a fixed rate - to limit stress on components. 15 | * Hood hard stops based on current draw. 16 | * 17 | */ 18 | public class MoveHoodButton extends CommandBase { 19 | 20 | private Shooter shooter; 21 | private boolean direction = Shooter.DOWN; 22 | 23 | public MoveHoodButton(Shooter shooter, boolean direction) { 24 | this.shooter = shooter; 25 | this.direction = direction; 26 | addRequirements(shooter); 27 | } 28 | 29 | // Called when the command is initially scheduled. 30 | @Override 31 | public void initialize() { 32 | // Start the hood motor. 33 | shooter.moveHood(direction); 34 | SmartDashboard.putNumber("Input Velocity", 0); 35 | } 36 | 37 | // Called every time the scheduler runs while the command is scheduled. 38 | @Override 39 | public void execute() { 40 | // Check if hard stop hit. Stop if so. 41 | if (shooter.checkHoodCurrentLimit()) { 42 | end(true); 43 | } 44 | 45 | if ((Shooter.UP == direction) && (ShooterConstants.kHoodMaxAngle < shooter.getHoodAngle()) 46 | || (Shooter.DOWN == direction) && (ShooterConstants.kHoodMinAngle > shooter.getHoodAngle())) { 47 | end(true); 48 | } 49 | double velocity = SmartDashboard.getNumber("Input Velocity", 0); 50 | shooter.setShooterVelocity(velocity); 51 | SmartDashboard.putNumber("Shooter Velocity", shooter.getShooterVelocity()); 52 | } 53 | 54 | // Called once the command ends or is interrupted. 55 | @Override 56 | public void end(boolean interrupted) { 57 | // Stop the motor 58 | double position = shooter.getHoodAngle(); 59 | SmartDashboard.putNumber("Target Position", position); 60 | shooter.setHoodPosition(position); 61 | 62 | } 63 | 64 | // Returns true when the command should end. 65 | @Override 66 | public boolean isFinished() { 67 | return false; 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/PresetFlywheelController.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj.Preferences; 4 | import frc.robot.shooter.Shooter; 5 | import frc.robot.status.Status; 6 | import frc.robot.vision.Limelight; 7 | 8 | public class PresetFlywheelController extends FlywheelController { 9 | 10 | private String preset; 11 | 12 | public PresetFlywheelController(Shooter shooter, String preset) { 13 | super(shooter, 0, 0); // Create with bogus speed and angle. Set real values in initialize. 14 | this.preset = preset; 15 | } 16 | 17 | @Override 18 | public void initialize() { 19 | switch (preset) { 20 | case "BUF": 21 | this.rpm = Preferences.getInt("BUF.Velocity", 1625); 22 | this.hoodAngle = Preferences.getDouble("BUF.Angle", 65.0); // baseline, upper goal, front shot 23 | break; 24 | case "BUR": 25 | this.rpm = Preferences.getInt("BUR.Velocity", 1625); 26 | this.hoodAngle = Preferences.getDouble("BUR.Angle", 65.0); // baseline, upper goal, rear shot 27 | break; 28 | case "TLR": 29 | this.rpm = Preferences.getInt("TLR.Velocity", 1625); 30 | this.hoodAngle = Preferences.getDouble("TLR.Angle", 65.0); // tarmac, lower goal, rear shot 31 | break; 32 | case "TUR": 33 | this.rpm = Preferences.getInt("TUR.Velocity", 1625); 34 | this.hoodAngle = Preferences.getDouble("TUR.Angle", 65.0); // tarmac, upper goal, rear shot 35 | break; 36 | case "BLP": 37 | this.rpm = Preferences.getInt("BLP.Velocity", 500); 38 | this.hoodAngle = Preferences.getDouble("BLP.Angle", 60.0); // Bloop shot 39 | break; 40 | case "SAF": 41 | this.rpm = Preferences.getInt("SAF.Velocity", 500); 42 | this.hoodAngle = Preferences.getDouble("SAF.Angle", 60.0); // safe loading zone shot 43 | break; 44 | default: 45 | this.rpm = 500; 46 | this.hoodAngle = 80.0; 47 | break; 48 | } 49 | shooter.setHoodPosition(this.hoodAngle); 50 | controller.startPeriodic(0.02); 51 | } 52 | 53 | @Override 54 | public void execute() { 55 | double targetDelta = rpm - velocity; 56 | 57 | if ((Math.abs(targetDelta) < 20) && !closeToTarget) { 58 | closeToTarget = true; 59 | if (!"BLP".equals(preset)) { 60 | Status.getInstance().fillLEDs(); 61 | } 62 | } 63 | } 64 | 65 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/TrajectoryFollower.java: -------------------------------------------------------------------------------- 1 | package frc.robot.drive.commands; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 6 | import edu.wpi.first.wpilibj.Timer; 7 | import edu.wpi.first.wpilibj2.command.CommandBase; 8 | import frc.lib.control.PIDController; 9 | import frc.lib.control.SwerveTrajectory; 10 | import frc.lib.control.SwerveTrajectory.State; 11 | import frc.paths.Path; 12 | import frc.robot.Constants.AutoConstants; 13 | import frc.robot.drive.Drivetrain; 14 | 15 | public class TrajectoryFollower extends CommandBase { 16 | private Drivetrain drive; 17 | private SwerveTrajectory trajectory; 18 | private PIDController xController, yController, thetaController; 19 | private double lastTime = 0; 20 | private Timer timer = new Timer(); 21 | 22 | public TrajectoryFollower(Drivetrain drive, Path path) { 23 | addRequirements(drive); 24 | this.drive = drive; 25 | this.trajectory = path.getPath(); 26 | } 27 | 28 | @Override 29 | public void initialize() { 30 | timer.reset(); 31 | timer.start(); 32 | 33 | xController = new PIDController(AutoConstants.kPTranslationController, 0, 0); 34 | yController = new PIDController(AutoConstants.kPTranslationController, 0, 0); 35 | thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0); 36 | thetaController.setContinous(true); 37 | thetaController.setInputRange(Math.PI * 2); 38 | 39 | lastTime = 0; 40 | } 41 | 42 | @Override 43 | public void execute() { 44 | double time = timer.get(); 45 | double dt = time - lastTime; 46 | State refState = trajectory.sample(time); 47 | Pose2d currentPose = drive.getPose(); 48 | 49 | xController.setReference(refState.pose.getX()); 50 | yController.setReference(refState.pose.getY()); 51 | thetaController.setReference(refState.pose.getRotation().getRadians()); 52 | 53 | double vx = xController.calculate(currentPose.getX(), dt) + refState.velocity.x; 54 | double vy = yController.calculate(currentPose.getY(), dt) + refState.velocity.y; 55 | double omega = -thetaController.calculate(currentPose.getRotation().getRadians(), dt) - refState.velocity.z; 56 | 57 | 58 | drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds( 59 | vx, 60 | vy, 61 | omega, 62 | drive.getHeading()), 63 | false); 64 | lastTime = time; 65 | } 66 | 67 | @Override 68 | public void end(boolean interrupted) { 69 | timer.stop(); 70 | drive.brake(); 71 | } 72 | 73 | @Override 74 | public boolean isFinished() { 75 | return timer.hasElapsed(trajectory.getTotalTime()); 76 | } 77 | } -------------------------------------------------------------------------------- /src/main/java/frc/lib/util/InterpolationTable.java: -------------------------------------------------------------------------------- 1 | package frc.lib.util; 2 | 3 | import java.util.Arrays; 4 | import java.util.Comparator; 5 | 6 | public class InterpolationTable { 7 | 8 | public static void main(String... args) { 9 | InterpolationTable table = new InterpolationTable(new double[][]{ 10 | {1.7, 1800, 78.25}, 11 | {2.4, 1875, 73.4}, 12 | {2.95, 1980, 73.25} 13 | }); 14 | System.out.println(Arrays.toString(table.sample(2))); 15 | } 16 | 17 | private static final Comparator INPUT_SORTER = (point1, point2) -> {return point1[0] > point2[0] ? 1 : -1;}; 18 | 19 | private final double[][] referencePoints; 20 | private final int propertyCount; 21 | 22 | public InterpolationTable(double[][] referencePoints) { 23 | Arrays.sort(referencePoints, INPUT_SORTER); 24 | this.referencePoints = referencePoints; 25 | propertyCount = referencePoints[0].length - 1; 26 | } 27 | 28 | public double[] sample(double input) { 29 | int leftBoundIndex = 0; 30 | int rightBoundIndex = referencePoints.length - 1; 31 | if (input < referencePoints[leftBoundIndex][0]) { 32 | double[] output = new double[propertyCount]; 33 | for (int i = 0; i < propertyCount; i++) { 34 | output[i] = referencePoints[leftBoundIndex][1 + i]; 35 | } 36 | return output; 37 | } 38 | if (input > referencePoints[rightBoundIndex][0]) { 39 | double[] output = new double[propertyCount]; 40 | for (int i = 0; i < propertyCount; i++) { 41 | output[i] = referencePoints[rightBoundIndex][1 + i]; 42 | } 43 | return output; 44 | } 45 | boolean found = false; 46 | while (!found) { 47 | int testIndex = (leftBoundIndex + rightBoundIndex) / 2; 48 | if (input < referencePoints[testIndex][0]) { 49 | rightBoundIndex = testIndex; 50 | } else { 51 | leftBoundIndex = testIndex; 52 | } 53 | if (rightBoundIndex - leftBoundIndex == 1) { 54 | found = true; 55 | } 56 | } 57 | double[] point1 = referencePoints[leftBoundIndex]; 58 | double[] point2 = referencePoints[rightBoundIndex]; 59 | double[] outputs = new double[propertyCount]; 60 | for (int i = 0; i < propertyCount; i++) { 61 | outputs[i] = interpolate(input, i, point1, point2); 62 | } 63 | return outputs; 64 | } 65 | 66 | private double interpolate(double input, int outputProperty, double[] point1, double[] point2) { 67 | outputProperty++; 68 | double slope = (point2[outputProperty] - point1[outputProperty]) / (point2[0] - point1[0]); 69 | double sample = input - point1[0]; 70 | double output = point1[outputProperty] + slope * sample; 71 | return output; 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/control/SwerveTrajectory.java: -------------------------------------------------------------------------------- 1 | package frc.lib.control; 2 | 3 | import java.util.ArrayList; 4 | import java.util.List; 5 | 6 | import edu.wpi.first.math.geometry.Pose2d; 7 | import edu.wpi.first.math.geometry.Rotation2d; 8 | import edu.wpi.first.math.geometry.Translation2d; 9 | 10 | public class SwerveTrajectory { 11 | private final List trajectory; 12 | 13 | public SwerveTrajectory(double[][] states) { 14 | trajectory = new ArrayList(); 15 | for (double[] state : states) { 16 | trajectory.add(new State( 17 | state[0], 18 | new Pose2d(new Translation2d(state[1], state[2]), new Rotation2d(state[3])), 19 | new Vector3d(state[4], state[5], state[6]) 20 | )); 21 | } 22 | } 23 | 24 | public Pose2d getInitialPose() { 25 | return trajectory.get(0).pose; 26 | } 27 | 28 | public double getTotalTime() { 29 | return trajectory.get(trajectory.size() - 1).t; 30 | } 31 | 32 | public State sample(double time) { 33 | if (time < trajectory.get(0).t) { 34 | return trajectory.get(0); 35 | } 36 | if (time > getTotalTime()) { 37 | return trajectory.get(trajectory.size() - 1); 38 | } 39 | 40 | int low = 1; 41 | int high = trajectory.size() - 1; 42 | 43 | while (low != high) { 44 | int mid = (low + high) / 2; 45 | if (trajectory.get(mid).t < time) { 46 | low = mid + 1; 47 | } else { 48 | high = mid; 49 | } 50 | } 51 | 52 | State previousState = trajectory.get(low - 1); 53 | State currentState = trajectory.get(low); 54 | 55 | if (currentState.t - previousState.t == 0) { 56 | return new State(currentState.t, currentState.pose, currentState.velocity); 57 | } 58 | 59 | return previousState.interpolate(currentState, (time - previousState.t) / (currentState.t - previousState.t)); 60 | } 61 | 62 | public static class State { 63 | public double t; 64 | public Pose2d pose; 65 | public Vector3d velocity; 66 | 67 | public State(double t, Pose2d pose, Vector3d velocity) { 68 | this.t = t; 69 | this.pose = pose; 70 | this.velocity = velocity; 71 | } 72 | 73 | public State interpolate(State end, double x) { 74 | double newT = (end.t - t) * x + t; 75 | Vector3d startPoseVector = Vector3d.fromPose(pose); 76 | Vector3d endPoseVector = Vector3d.fromPose(end.pose); 77 | Pose2d newPose = (((endPoseVector.minus(startPoseVector)).times(x)).plus(startPoseVector)).toPose(); 78 | Vector3d newVelocity = ((end.velocity.minus(velocity)).times(x)).plus(velocity); 79 | return new State( 80 | newT, 81 | newPose, 82 | newVelocity 83 | ); 84 | } 85 | } 86 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/FlywheelController.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj.Notifier; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | import edu.wpi.first.wpilibj2.command.CommandBase; 7 | import edu.wpi.first.wpilibj2.command.PrintCommand; 8 | import edu.wpi.first.wpilibj2.command.ScheduleCommand; 9 | import frc.lib.control.DCMotor; 10 | import frc.lib.control.PIDController; 11 | import frc.robot.shooter.Shooter; 12 | import frc.robot.status.Status; 13 | import frc.robot.status.commands.FillLEDsCommand; 14 | 15 | public class FlywheelController extends CommandBase { 16 | 17 | Shooter shooter; 18 | double lastPosition, lastTime, hoodAngle; 19 | double rpm; 20 | private Timer timer = new Timer(); 21 | Notifier controller = new Notifier(this::controller); 22 | private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.37); 23 | private PIDController flywheelController = new PIDController(0.0125, 0, 0); 24 | // private PIDController flywheelController = new PIDController(0.05, 0, 0); 25 | boolean closeToTarget = false; 26 | double velocity = 0; 27 | 28 | public FlywheelController(Shooter shooter, double rpm, double hoodAngle) { 29 | this.shooter = shooter; 30 | this.rpm = rpm; 31 | this.hoodAngle = hoodAngle; 32 | lastPosition = shooter.getEncoderPosition(); 33 | lastTime = 0; 34 | timer.reset(); 35 | timer.start(); 36 | addRequirements(shooter); 37 | } 38 | 39 | @Override 40 | public void initialize() { 41 | shooter.setHoodPosition(hoodAngle); 42 | closeToTarget = false; 43 | velocity = 0; 44 | lastPosition = shooter.getEncoderPosition(); 45 | timer.reset(); 46 | lastTime = 0; 47 | // timer.start(); 48 | controller.startPeriodic(0.02); 49 | } 50 | 51 | void controller() { 52 | double time = timer.get(); 53 | double position = shooter.getEncoderPosition(); 54 | double dt = time - lastTime; 55 | if (dt < 0.0001) { 56 | dt = 0.001; 57 | } 58 | velocity = (position - lastPosition) / (dt) * 60; 59 | 60 | flywheelController.setReference(rpm); 61 | SmartDashboard.putNumber("Estimated velocity", velocity); 62 | 63 | double voltage = flywheel.solveFeedforward(rpm, 0) + flywheelController.calculate(velocity, dt); 64 | // double voltage = flywheel.solveFeedforward(rpm, 0); 65 | 66 | voltage = Math.min(voltage, 11.5); 67 | 68 | shooter.setShooterVoltage(voltage); 69 | 70 | lastPosition = position; 71 | lastTime = time; 72 | } 73 | 74 | @Override 75 | public void end(boolean interrupted) { 76 | controller.stop(); 77 | closeToTarget= false; 78 | velocity = 0; 79 | } 80 | 81 | @Override 82 | public boolean isFinished() { 83 | return false; 84 | } 85 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/LedAction.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | import edu.wpi.first.wpilibj.AddressableLEDBuffer; 4 | import frc.robot.status.Status; 5 | 6 | public class LedAction extends Action { 7 | 8 | // How many times to run, less than 0 will run forever. 9 | protected int intervalCount = 1; 10 | protected int curIntCount = 1; // The current interval. 11 | 12 | // Buffer this action uses for sending to the LEDs. 13 | protected AddressableLEDBuffer buffer = new AddressableLEDBuffer(Status.ADDRESSABLE_LED_COUNT); 14 | 15 | public LedAction() { 16 | // Initialize the buffer to black. 17 | for (var i = 0; i < buffer.getLength(); i++) { 18 | buffer.setRGB(i, 0, 0, 0); 19 | } 20 | } 21 | 22 | // Invoke with a specific color. 23 | public LedAction(int red, int green, int blue, int brightness) { 24 | double b = brightness / 255.0; 25 | 26 | red = (int) (red * b); 27 | green = (int) (green * b); 28 | blue = (int) (blue * b); 29 | 30 | // Set the entire buffer (string of leds) to the same color. 31 | for (var i = 0; i < buffer.getLength(); i++) { 32 | buffer.setRGB(i, red, green, blue); 33 | } 34 | } 35 | 36 | public void setIntervalCount(int intervalCount) { 37 | this.intervalCount = intervalCount; 38 | this.curIntCount = intervalCount; 39 | } 40 | 41 | public int getIntervalCount() { 42 | return this.intervalCount; 43 | } 44 | 45 | public int getCurrentIntervalCount() { 46 | return this.curIntCount; 47 | } 48 | 49 | // Implementations should override the updateBuffer method. 50 | // This will be invoked every intervalTime seconds and only needs to 51 | // alter the buffer. The outer run() method will handle the intervalTime, 52 | // intervalCount, and set/sending the buffer to the LEDs. 53 | protected void updateBuffer() { 54 | 55 | } 56 | 57 | @Override 58 | public boolean isFinished() { 59 | // If the intervalCount is greater than 0 or less than zero 60 | // we're not finished. The run() method will decrement if greater 61 | // and never finish if less than (a repeating pattern). 62 | if (curIntCount == 0) { 63 | return true; 64 | } else { 65 | return false; 66 | } 67 | } 68 | 69 | @Override 70 | public void reset() { 71 | this.curIntCount = this.intervalCount; 72 | } 73 | 74 | 75 | // Require by the parent Action class. 76 | // This is invoked by the running thread until isFinished returns true. 77 | @Override 78 | public void run() { 79 | 80 | // Update the buffer. 81 | updateBuffer(); 82 | 83 | // Send the buffer to the leds. 84 | Status.getInstance().setLedData(buffer); 85 | 86 | // Only decrement the intervalCount if it's over 0. 87 | // Otherwise it may overflow backwords and cause problems. 88 | if (curIntCount > 0) { 89 | --curIntCount; 90 | } 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/SuperRudeAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | 5 | import javax.swing.GroupLayout.ParallelGroup; 6 | 7 | import edu.wpi.first.math.geometry.Pose2d; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.geometry.Translation2d; 10 | import edu.wpi.first.wpilibj.util.Color; 11 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 12 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 13 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 14 | import frc.paths.GoForwardHalfMeter; 15 | import frc.paths.OnePointEightMetersForward; 16 | import frc.paths.ShootTwoBalls; 17 | import frc.paths.SuperRude; 18 | import frc.paths.TwoBallPartOne; 19 | import frc.paths.TwoBallPartTwo; 20 | import frc.robot.drive.Drivetrain; 21 | import frc.robot.drive.commands.ResetOdometry; 22 | import frc.robot.drive.commands.TrajectoryFollower; 23 | import frc.robot.intake.Intake; 24 | import frc.robot.intake.commands.FastIntake; 25 | import frc.robot.intake.commands.RetractIntake; 26 | import frc.robot.shooter.Shooter; 27 | import frc.robot.shooter.commands.FlywheelController; 28 | import frc.robot.shooter.commands.PullTrigger; 29 | import frc.robot.shooter.commands.ResetHood; 30 | import frc.robot.shooter.commands.SetShooterState; 31 | import frc.robot.shooter.commands.StopShooter; 32 | import frc.robot.shooter.commands.StopTrigger; 33 | import frc.robot.status.Status; 34 | import frc.robot.status.actions.ImageAction; 35 | import frc.robot.status.commands.ActionCommand; 36 | import frc.robot.status.commands.SetColor; 37 | 38 | public class SuperRudeAuto extends SequentialCommandGroup{ 39 | public SuperRudeAuto(Drivetrain drive, Intake intake, Shooter shooter) { 40 | addCommands( 41 | new ResetOdometry(drive, new Pose2d(0,0,new Rotation2d(2.32))), 42 | new ParallelDeadlineGroup( 43 | new TrajectoryFollower(drive, new TwoBallPartOne()), 44 | new FastIntake(intake) 45 | ), 46 | new ParallelDeadlineGroup( // Shoot the balls 47 | new SequentialCommandGroup( 48 | new WaitCommand(1.5), // Give shooter time to spin up & hood to move 49 | new PullTrigger(shooter, intake), 50 | new WaitCommand(2)), 51 | new FlywheelController(shooter, 1735, 81)), 52 | // new RetractIntake(intake)), 53 | new StopTrigger(shooter, intake), 54 | new StopShooter(shooter), 55 | new ParallelDeadlineGroup( 56 | new TrajectoryFollower(drive, new SuperRude()), 57 | new FastIntake(intake)), 58 | new ParallelDeadlineGroup( // Toss red ball away 59 | new SequentialCommandGroup( 60 | new WaitCommand(0.5), // Give shooter time to spin up & hood to move 61 | new PullTrigger(shooter, intake), 62 | new WaitCommand(2)), 63 | new FlywheelController(shooter, 550, 60), 64 | new RetractIntake(intake)) 65 | ); 66 | } 67 | } -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Resolve any "." and ".." in APP_HOME to make it shorter. 33 | for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi 34 | 35 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 36 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 37 | 38 | @rem Find java.exe 39 | if defined JAVA_HOME goto findJavaFromJavaHome 40 | 41 | set JAVA_EXE=java.exe 42 | %JAVA_EXE% -version >NUL 2>&1 43 | if "%ERRORLEVEL%" == "0" goto execute 44 | 45 | echo. 46 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 47 | echo. 48 | echo Please set the JAVA_HOME variable in your environment to match the 49 | echo location of your Java installation. 50 | 51 | goto fail 52 | 53 | :findJavaFromJavaHome 54 | set JAVA_HOME=%JAVA_HOME:"=% 55 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 56 | 57 | if exist "%JAVA_EXE%" goto execute 58 | 59 | echo. 60 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 61 | echo. 62 | echo Please set the JAVA_HOME variable in your environment to match the 63 | echo location of your Java installation. 64 | 65 | goto fail 66 | 67 | :execute 68 | @rem Setup the command line 69 | 70 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 71 | 72 | 73 | @rem Execute Gradle 74 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* 75 | 76 | :end 77 | @rem End local scope for the variables with windows NT shell 78 | if "%ERRORLEVEL%"=="0" goto mainEnd 79 | 80 | :fail 81 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 82 | rem the _cmd.exe /c_ return code! 83 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 84 | exit /b 1 85 | 86 | :mainEnd 87 | if "%OS%"=="Windows_NT" endlocal 88 | 89 | :omega 90 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/SecondSuperRudeAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | 5 | import javax.swing.GroupLayout.ParallelGroup; 6 | 7 | import edu.wpi.first.math.geometry.Pose2d; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.geometry.Translation2d; 10 | import edu.wpi.first.wpilibj.util.Color; 11 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 12 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 13 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 14 | import frc.paths.GoForwardHalfMeter; 15 | import frc.paths.OnePointEightMetersForward; 16 | import frc.paths.ShootTwoBalls; 17 | import frc.paths.SuperRude; 18 | import frc.paths.SuperRudeTwo; 19 | import frc.paths.TwoBallPartOne; 20 | import frc.paths.TwoBallPartTwo; 21 | import frc.robot.drive.Drivetrain; 22 | import frc.robot.drive.commands.ResetOdometry; 23 | import frc.robot.drive.commands.TrajectoryFollower; 24 | import frc.robot.intake.Intake; 25 | import frc.robot.intake.commands.FastIntake; 26 | import frc.robot.intake.commands.RetractIntake; 27 | import frc.robot.shooter.Shooter; 28 | import frc.robot.shooter.commands.FlywheelController; 29 | import frc.robot.shooter.commands.PullTrigger; 30 | import frc.robot.shooter.commands.ResetHood; 31 | import frc.robot.shooter.commands.SetShooterState; 32 | import frc.robot.shooter.commands.StopShooter; 33 | import frc.robot.shooter.commands.StopTrigger; 34 | import frc.robot.status.Status; 35 | import frc.robot.status.actions.ImageAction; 36 | import frc.robot.status.commands.ActionCommand; 37 | import frc.robot.status.commands.SetColor; 38 | 39 | public class SecondSuperRudeAuto extends SequentialCommandGroup{ 40 | public SecondSuperRudeAuto(Drivetrain drive, Intake intake, Shooter shooter) { 41 | addCommands( 42 | new ResetOdometry(drive, new Pose2d(0,0,new Rotation2d(2.32))), 43 | new ParallelDeadlineGroup( 44 | new WaitCommand(4.5), 45 | new SequentialCommandGroup( 46 | new WaitCommand(1.0), 47 | new FlywheelController(shooter, 1735, 79) 48 | ), 49 | new SequentialCommandGroup( 50 | new WaitCommand(2.5), // Give shooter time to spin up & hood to move 51 | new PullTrigger(shooter, intake)), 52 | new TrajectoryFollower(drive, new TwoBallPartOne()), 53 | new FastIntake(intake) 54 | ), 55 | new StopTrigger(shooter, intake), 56 | new StopShooter(shooter), 57 | new ParallelDeadlineGroup( 58 | new TrajectoryFollower(drive, new SuperRudeTwo()), 59 | new FastIntake(intake)), 60 | new ParallelDeadlineGroup( // Toss red ball away 61 | new SequentialCommandGroup( 62 | new WaitCommand(0.5), // Give shooter time to spin up & hood to move 63 | new PullTrigger(shooter, intake), 64 | new WaitCommand(2)), 65 | new FlywheelController(shooter, 550, 60), 66 | new RetractIntake(intake)) 67 | ); 68 | } 69 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/drive/commands/MotionProfileTurn.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.drive.commands; 6 | 7 | import edu.wpi.first.math.controller.ProfiledPIDController; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 10 | import edu.wpi.first.math.trajectory.TrapezoidProfile; 11 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 12 | import edu.wpi.first.wpilibj2.command.CommandBase; 13 | import frc.robot.drive.Drivetrain; 14 | 15 | public class MotionProfileTurn extends CommandBase { 16 | Drivetrain drive; 17 | double offset; 18 | double target; 19 | double velocity; 20 | double dt; 21 | double maxAcceleration, maxVelocity; 22 | ProfiledPIDController controller; 23 | 24 | public MotionProfileTurn(Drivetrain drive, double offset) { 25 | this.drive = drive; 26 | this.offset = offset; 27 | addRequirements(drive); 28 | } 29 | 30 | @Override 31 | public void initialize() { 32 | // target = offset + drive.getPose().getRotation().getRadians(); 33 | SmartDashboard.putString("Turning", "Enabled"); 34 | controller = new ProfiledPIDController(0.35, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 1), 0.02); 35 | // controller.enableContinuousInput(-Math.PI, Math.PI); 36 | velocity = 0; 37 | maxAcceleration = 0.5; 38 | maxVelocity = 1; 39 | dt = 0.02; 40 | } 41 | 42 | @Override 43 | public void execute() { 44 | double theta = drive.getPose().getRotation().getRadians(); 45 | double error = shortAngle(0, theta); 46 | double feedback = controller.calculate(0, shortAngle(0, theta)); 47 | double feedforward = solveVelocity(maxVelocity, maxAcceleration, error, dt); 48 | SmartDashboard.putNumber("Error", error); 49 | SmartDashboard.putNumber("Velocity", feedforward); 50 | drive.drive(new ChassisSpeeds(0, 0, feedforward), false); 51 | } 52 | 53 | public double solveVelocity(double maxVelocity, double maxAcceleration, double error, double dt) { 54 | double accel = 0; 55 | if (Math.sqrt(2*maxAcceleration*Math.abs(error)) > Math.abs(velocity)) { 56 | accel = -maxAcceleration * Math.signum(error); 57 | } else { 58 | accel = maxAcceleration * Math.signum(error); 59 | } 60 | SmartDashboard.putNumber("Accel", accel); 61 | velocity = Math.min(maxVelocity, Math.max(-maxVelocity, velocity + accel * dt)); 62 | // velocity += accel * dt; 63 | return velocity; 64 | } 65 | 66 | public double shortAngle(double targetAngle, double currentAngle) { 67 | double diff = (targetAngle - currentAngle + Math.PI) % (2* Math.PI) - Math.PI; 68 | diff = diff < -Math.PI ? diff + 2 * Math.PI : diff; 69 | return diff; 70 | } 71 | 72 | // Called once the command ends or is interrupted. 73 | @Override 74 | public void end(boolean interrupted) { 75 | drive.brake(); 76 | SmartDashboard.putString("Turning", "Disabled"); 77 | } 78 | 79 | // Returns true when the command should end. 80 | @Override 81 | public boolean isFinished() { 82 | return false; 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/TwoBallEastAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Translation2d; 6 | import edu.wpi.first.wpilibj.util.Color; 7 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 8 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 9 | import edu.wpi.first.wpilibj2.command.WaitCommand; 10 | import frc.paths.WeirdAutoPartOne; 11 | import frc.paths.WeirdAutoPartTwo; 12 | import frc.robot.drive.Drivetrain; 13 | import frc.robot.drive.commands.ResetOdometry; 14 | import frc.robot.drive.commands.TrajectoryFollower; 15 | import frc.robot.intake.Intake; 16 | import frc.robot.intake.commands.FastIntake; 17 | import frc.robot.intake.commands.RetractIntake; 18 | import frc.robot.shooter.Shooter; 19 | import frc.robot.shooter.commands.FlywheelController; 20 | import frc.robot.shooter.commands.PullTrigger; 21 | import frc.robot.shooter.commands.ResetHood; 22 | import frc.robot.shooter.commands.StopShooter; 23 | import frc.robot.shooter.commands.StopTrigger; 24 | import frc.robot.status.Status; 25 | import frc.robot.status.actions.ImageAction; 26 | import frc.robot.status.commands.ActionCommand; 27 | import frc.robot.status.commands.SetColor; 28 | 29 | public class TwoBallEastAuto extends SequentialCommandGroup{ 30 | public TwoBallEastAuto(Drivetrain drive, Intake intake, Shooter shooter) { 31 | addCommands( 32 | new ActionCommand(new ImageAction("THfade.png", 0.01).oscillate().brightness(0.7)), 33 | new ResetOdometry(drive, new Pose2d(new Translation2d(0,0),Rotation2d.fromDegrees(-90.0))), 34 | new ParallelDeadlineGroup( // Pick up ball 35 | new TrajectoryFollower(drive, new WeirdAutoPartOne()), 36 | new ResetHood(shooter), // Reset the hood while moving to pick up ball 37 | new FastIntake(intake)), 38 | new ParallelDeadlineGroup( // Shoot two balls 39 | new SequentialCommandGroup( 40 | new WaitCommand(2), // Give shooter time to spin up & hood to move 41 | new PullTrigger(shooter, intake), 42 | new WaitCommand(2)), 43 | new FlywheelController(shooter, 1800, 78.25), 44 | new RetractIntake(intake)), 45 | new StopTrigger(shooter, intake), 46 | new StopShooter(shooter), 47 | new ParallelDeadlineGroup( // Pick up red ball 48 | new TrajectoryFollower(drive, new WeirdAutoPartTwo()), 49 | new FastIntake(intake)), 50 | new WaitCommand(1.5), 51 | new ParallelDeadlineGroup( // Toss red ball away 52 | new SequentialCommandGroup( 53 | new WaitCommand(1), 54 | new PullTrigger(shooter, intake), 55 | new WaitCommand(1)), 56 | new FlywheelController(shooter, 1000, 65), 57 | new RetractIntake(intake)), 58 | new StopShooter(shooter), 59 | new StopTrigger(shooter, intake), 60 | new ResetHood(shooter), 61 | new SetColor(Status.getInstance(), Color.kBlack) 62 | ); 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # This gitignore has been specially created by the WPILib team. 2 | # If you remove items from this file, intellisense might break. 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | ### Java ### 39 | # Compiled class file 40 | *.class 41 | 42 | # Log file 43 | *.log 44 | 45 | # BlueJ files 46 | *.ctxt 47 | 48 | # Mobile Tools for Java (J2ME) 49 | .mtj.tmp/ 50 | 51 | # Package Files # 52 | *.jar 53 | *.war 54 | *.nar 55 | *.ear 56 | *.zip 57 | *.tar.gz 58 | *.rar 59 | 60 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 61 | hs_err_pid* 62 | 63 | ### Linux ### 64 | *~ 65 | 66 | # temporary files which can be created if a process still has a handle open of a deleted file 67 | .fuse_hidden* 68 | 69 | # KDE directory preferences 70 | .directory 71 | 72 | # Linux trash folder which might appear on any partition or disk 73 | .Trash-* 74 | 75 | # .nfs files are created when an open file is removed but is still being accessed 76 | .nfs* 77 | 78 | ### macOS ### 79 | # General 80 | .DS_Store 81 | .AppleDouble 82 | .LSOverride 83 | 84 | # Icon must end with two \r 85 | Icon 86 | 87 | # Thumbnails 88 | ._* 89 | 90 | # Files that might appear in the root of a volume 91 | .DocumentRevisions-V100 92 | .fseventsd 93 | .Spotlight-V100 94 | .TemporaryItems 95 | .Trashes 96 | .VolumeIcon.icns 97 | .com.apple.timemachine.donotpresent 98 | 99 | # Directories potentially created on remote AFP share 100 | .AppleDB 101 | .AppleDesktop 102 | Network Trash Folder 103 | Temporary Items 104 | .apdisk 105 | 106 | ### VisualStudioCode ### 107 | .vscode 108 | .vscode/* 109 | !.vscode/settings.json 110 | !.vscode/tasks.json 111 | !.vscode/launch.json 112 | !.vscode/extensions.json 113 | 114 | ### Windows ### 115 | # Windows thumbnail cache files 116 | Thumbs.db 117 | ehthumbs.db 118 | ehthumbs_vista.db 119 | 120 | # Dump file 121 | *.stackdump 122 | 123 | # Folder config file 124 | [Dd]esktop.ini 125 | 126 | # Recycle Bin used on file shares 127 | $RECYCLE.BIN/ 128 | 129 | # Windows Installer files 130 | *.cab 131 | *.msi 132 | *.msix 133 | *.msm 134 | *.msp 135 | 136 | # Windows shortcuts 137 | *.lnk 138 | 139 | ### Gradle ### 140 | .gradle 141 | /build/ 142 | 143 | # Ignore Gradle GUI config 144 | gradle-app.setting 145 | 146 | # Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) 147 | !gradle-wrapper.jar 148 | 149 | # Cache of project 150 | .gradletasknamecache 151 | 152 | # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 153 | # gradle/wrapper/gradle-wrapper.properties 154 | 155 | # # VS Code Specific Java Settings 156 | # DO NOT REMOVE .classpath and .project 157 | .classpath 158 | .project 159 | .settings/ 160 | bin/ 161 | src/main/python/__pycache__ 162 | 163 | # Simulation GUI and other tools window save file 164 | *-window.json 165 | 166 | build/ 167 | 168 | simgui-ds.json 169 | simgui.json 170 | networktables.ini 171 | .SysId 172 | 173 | .pyc 174 | .bak -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/actions/ScannerAction.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.actions; 2 | 3 | import edu.wpi.first.wpilibj.util.Color; 4 | import edu.wpi.first.wpilibj.util.Color8Bit; 5 | 6 | public class ScannerAction extends LedAction { 7 | 8 | // State 9 | private int leadingIndex = 0; 10 | private boolean forward = true; 11 | private double chaseFactor = 0.10; 12 | 13 | private int red = 0; 14 | private int green = 0; 15 | private int blue = 0; 16 | private int brightness = 0; 17 | 18 | // Default will run a red pattern. 19 | public ScannerAction() { 20 | super(); 21 | 22 | red = 255; 23 | brightness = 100; 24 | 25 | // Run forever, 10ms 26 | intervalCount = -1; 27 | intervalTime = 0.010; 28 | } 29 | 30 | public ScannerAction(Color color, int brightness, double chaseFactor, double intervalTime) { 31 | this(new Color8Bit(color).red, new Color8Bit(color).green, new Color8Bit(color).blue, brightness); 32 | this.chaseFactor = chaseFactor; 33 | this.intervalTime = intervalTime; 34 | } 35 | 36 | public ScannerAction(int red, int green, int blue, int brightness) { 37 | super(); 38 | 39 | this.red = red; 40 | this.green = green; 41 | this.blue = blue; 42 | this.brightness = brightness; 43 | 44 | // Run forever, 10ms 45 | intervalCount = -1; 46 | intervalTime = 0.010; 47 | } 48 | 49 | protected void updateBuffer() { 50 | 51 | int r = (int) (red * (brightness / 255.0)); 52 | int g = (int) (green * (brightness / 255.0)); 53 | int b = (int) (blue * (brightness / 255.0)); 54 | 55 | int chaseCount = (int) (buffer.getLength() * chaseFactor); 56 | 57 | for (var i = 0; i < buffer.getLength(); i++) { 58 | 59 | if (i == leadingIndex) { 60 | // The leading pixel. 61 | buffer.setRGB(i, r, g, b); 62 | } else { 63 | 64 | int d = 0; // How many pixels we are from the leading pixel 65 | 66 | if (forward) { 67 | if ((i < leadingIndex) && (i > leadingIndex - chaseCount)) { 68 | d = leadingIndex - i; 69 | } 70 | } else { 71 | if ((i > leadingIndex) && (i < leadingIndex + chaseCount)) { 72 | d = i - leadingIndex; 73 | } 74 | } 75 | 76 | if (d != 0) { 77 | double fade = 1.0 - ((double)d / (double)chaseCount); 78 | 79 | int cr = (int) (r * fade); 80 | int cg = (int) (g * fade); 81 | int cb = (int) (b * fade); 82 | 83 | buffer.setRGB(i, cr, cg, cb); 84 | } else { 85 | buffer.setRGB(i, 0, 0, 0); 86 | } 87 | } 88 | } 89 | 90 | if (forward == true) { 91 | ++leadingIndex; 92 | if (leadingIndex == buffer.getLength()) { 93 | forward = false; 94 | --leadingIndex; 95 | } 96 | } else { 97 | --leadingIndex; 98 | if (leadingIndex < 0) { 99 | leadingIndex = 0; 100 | forward = true; 101 | } 102 | } 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/status/commands/IdleCommand.java: -------------------------------------------------------------------------------- 1 | package frc.robot.status.commands; 2 | 3 | import edu.wpi.first.wpilibj.DriverStation; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj.DriverStation.Alliance; 6 | import edu.wpi.first.wpilibj.DriverStation.MatchType; 7 | import edu.wpi.first.wpilibj.util.Color; 8 | import edu.wpi.first.wpilibj2.command.CommandBase; 9 | import edu.wpi.first.wpilibj2.command.PrintCommand; 10 | import frc.robot.Robot; 11 | import frc.robot.status.Status; 12 | import frc.robot.status.actions.Action; 13 | import frc.robot.status.actions.ChaseAction; 14 | import frc.robot.status.actions.ImageAction; 15 | import frc.robot.status.actions.ScannerAction; 16 | 17 | /** 18 | * This is the command for the robot to run when no buttons 19 | * are pressed during teleop. 20 | * 21 | * It is a simple wrapper around any other Status command. 22 | * 23 | * This is not the "default" command. 24 | */ 25 | public class IdleCommand extends CommandBase { 26 | 27 | private static final double TELEOP_LENGTH = 135.0; 28 | private Action scannerAction; 29 | private Status status; 30 | private boolean actionSet; 31 | private int phase = 0; 32 | private Action twentySecondsAction = new ImageAction(Robot.twentySecondImage,0.05, ImageAction.FOREVER); 33 | private Action tenSecondsAction = new ImageAction(Robot.tenSecondImage, 0.15, ImageAction.FOREVER); 34 | 35 | public IdleCommand() { 36 | this.status = Status.getInstance(); 37 | addRequirements(status); 38 | } 39 | 40 | @Override 41 | public void initialize() { 42 | actionSet = false; 43 | phase = 0; 44 | if (Alliance.Red == DriverStation.getAlliance()) { 45 | scannerAction = new ScannerAction(Color.kMaroon, 255, 1.0, 0.05); 46 | } else if (Alliance.Blue == DriverStation.getAlliance()) { 47 | scannerAction = new ScannerAction(Color.kDarkBlue, 255, 1.0, 0.05); 48 | } else { 49 | scannerAction = new ScannerAction(Color.kDarkOrchid, 200, 1.0, 0.05); 50 | } 51 | } 52 | 53 | @Override 54 | public void execute() { 55 | /** 56 | getMatchTime() is problematic. 57 | During a real match, it counts DOWN. 58 | But when attached to the simulator, or when in free drive TeleOp mode it counts UP. 59 | This makes it difficult to reliably use it during development. 60 | 61 | Do some gymnastics to try to generate an elapsed time from getMatchTime(). 62 | **/ 63 | double timeElapsed = 0; 64 | timeElapsed = Timer.getFPGATimestamp() - status.getTeleopStartTime(); 65 | 66 | Action action = scannerAction; 67 | // new PrintCommand("Time Left = " + String.valueOf(timeElapsed)).schedule(); 68 | if ( 115.0 < timeElapsed && (0 == phase)) { // 20 Seconds left 69 | action = twentySecondsAction; 70 | phase = 1; 71 | actionSet = false; 72 | } 73 | 74 | if (125.0 < timeElapsed && 1 == phase) { // Last 10 seconds 75 | action = tenSecondsAction; 76 | phase = 2; 77 | actionSet = false; 78 | } 79 | 80 | if (!actionSet) { 81 | status.setAction(action); 82 | actionSet = true; 83 | } 84 | } 85 | 86 | @Override 87 | public void end(boolean interrupted) { 88 | } 89 | 90 | @Override 91 | public boolean isFinished() { 92 | return false; 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/TwoBallSouthAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | 5 | import javax.swing.GroupLayout.ParallelGroup; 6 | 7 | import edu.wpi.first.math.geometry.Pose2d; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.geometry.Translation2d; 10 | import edu.wpi.first.wpilibj.util.Color; 11 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 12 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 13 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 14 | import frc.paths.GoForwardHalfMeter; 15 | import frc.paths.OnePointEightMetersForward; 16 | import frc.paths.ShootTwoBalls; 17 | import frc.paths.TwoBallPartOne; 18 | import frc.paths.TwoBallPartTwo; 19 | import frc.robot.drive.Drivetrain; 20 | import frc.robot.drive.commands.ResetOdometry; 21 | import frc.robot.drive.commands.TrajectoryFollower; 22 | import frc.robot.intake.Intake; 23 | import frc.robot.intake.commands.FastIntake; 24 | import frc.robot.intake.commands.RetractIntake; 25 | import frc.robot.shooter.Shooter; 26 | import frc.robot.shooter.commands.FlywheelController; 27 | import frc.robot.shooter.commands.PullTrigger; 28 | import frc.robot.shooter.commands.ResetHood; 29 | import frc.robot.shooter.commands.SetShooterState; 30 | import frc.robot.shooter.commands.StopShooter; 31 | import frc.robot.shooter.commands.StopTrigger; 32 | import frc.robot.status.Status; 33 | import frc.robot.status.actions.ImageAction; 34 | import frc.robot.status.commands.ActionCommand; 35 | import frc.robot.status.commands.SetColor; 36 | 37 | public class TwoBallSouthAuto extends SequentialCommandGroup{ 38 | public TwoBallSouthAuto(Drivetrain drive, Intake intake, Shooter shooter) { 39 | addCommands( 40 | new ActionCommand(new ImageAction("noisy-pulse.png", 0.25).brightness(0.7)), 41 | new ResetOdometry(drive, new Pose2d(new Translation2d(0,0),new Rotation2d(2.32))), 42 | new ParallelDeadlineGroup( // Pick up blue ball 43 | new TrajectoryFollower(drive, new TwoBallPartOne()), 44 | new ResetHood(shooter), 45 | new FastIntake(intake)), 46 | new ParallelDeadlineGroup( // Shoot the balls 47 | new SequentialCommandGroup( 48 | new WaitCommand(1.5), // Give shooter time to spin up & hood to move 49 | new PullTrigger(shooter, intake), 50 | new WaitCommand(2)), 51 | new FlywheelController(shooter, 1725, 81), 52 | new RetractIntake(intake)), 53 | new StopTrigger(shooter, intake), 54 | new StopShooter(shooter), 55 | new ParallelDeadlineGroup( // Pick up red ball 56 | new WaitCommand(4.0), 57 | new FastIntake(intake), 58 | new TrajectoryFollower(drive, new TwoBallPartTwo())), 59 | new ParallelDeadlineGroup( // Toss red ball away 60 | new SequentialCommandGroup( 61 | new WaitCommand(1.5), // Give shooter time to spin up & hood to move 62 | new PullTrigger(shooter, intake), 63 | new WaitCommand(2)), 64 | new FlywheelController(shooter, 550, 60), 65 | new RetractIntake(intake)), 66 | new StopShooter(shooter), 67 | new StopTrigger(shooter, intake), 68 | new ResetHood(shooter), 69 | new SetColor(Status.getInstance(), Color.kBlack) 70 | ); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/shooter/commands/VisionShooter.java: -------------------------------------------------------------------------------- 1 | package frc.robot.shooter.commands; 2 | 3 | import edu.wpi.first.wpilibj.Notifier; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | import edu.wpi.first.wpilibj.util.Color; 7 | import edu.wpi.first.wpilibj2.command.CommandBase; 8 | import frc.lib.control.DCMotor; 9 | import frc.lib.control.PIDController; 10 | import frc.robot.shooter.Shooter; 11 | import frc.robot.status.Status; 12 | import frc.robot.vision.Limelight; 13 | 14 | public class VisionShooter extends CommandBase { 15 | 16 | Shooter shooter; 17 | Limelight limelight; 18 | double lastPosition, lastTime, hoodAngle; 19 | double distance; 20 | double rpm, rpmDelta; 21 | Timer timer = new Timer(); 22 | Notifier controller = new Notifier(this::controller); 23 | DCMotor flywheel = new DCMotor(0.002081897, 0, 0.302); 24 | PIDController flywheelController = new PIDController(0.0125, 0, 0); 25 | boolean closeToTarget = false; 26 | double velocity = 0; 27 | 28 | public VisionShooter(Shooter shooter, Limelight limelight) { 29 | this.shooter = shooter; 30 | this.limelight = limelight; 31 | this.lastPosition = shooter.getEncoderPosition(); 32 | this.lastTime = 0; 33 | this.timer.reset(); 34 | this.timer.start(); 35 | addRequirements(shooter); 36 | } 37 | 38 | @Override 39 | public void initialize() { 40 | 41 | closeToTarget = false; 42 | velocity = 0; 43 | lastPosition = shooter.getEncoderPosition(); 44 | timer.reset(); 45 | lastTime = 0; 46 | // timer.start(); 47 | controller.startPeriodic(0.02); 48 | limelight.turnOnLEDs(); 49 | } 50 | 51 | void controller() { 52 | double time = timer.get(); 53 | double position = shooter.getEncoderPosition(); 54 | double dt = time - lastTime; 55 | if (dt < 0.0001) { 56 | dt = 0.001; 57 | } 58 | velocity = (position - lastPosition) / (dt) * 60; 59 | 60 | distance = limelight.getState().distance; 61 | rpm = Shooter.lookupVelocity(distance); 62 | hoodAngle = Shooter.lookupHoodAngle(distance); 63 | 64 | shooter.setHoodPosition(hoodAngle); 65 | flywheelController.setReference(rpm); 66 | 67 | double voltage = flywheel.solveFeedforward(rpm, 0) + flywheelController.calculate(velocity, dt); 68 | // double voltage = flywheel.solveFeedforward(rpm, 0); 69 | 70 | shooter.setShooterVoltage(Math.min(voltage, 11.5)); 71 | 72 | lastPosition = position; 73 | lastTime = time; 74 | } 75 | 76 | @Override 77 | public void execute() { 78 | rpmDelta = Math.abs(rpm - velocity); 79 | 80 | SmartDashboard.putNumber("Estimated velocity", velocity); 81 | SmartDashboard.putNumber("Vision RPM Delta", rpmDelta); 82 | 83 | if (rpmDelta < 50 ) { 84 | closeToTarget = true; 85 | Status.getInstance().fillLEDs(); 86 | } else if (rpmDelta > 100) { 87 | closeToTarget = false; 88 | Color cur_color = Status.getInstance().getLedData().getLED(0); 89 | Status.getInstance().setColor(cur_color, 255, 50); 90 | } 91 | } 92 | 93 | @Override 94 | public void end(boolean interrupted) { 95 | controller.stop(); 96 | closeToTarget= false; 97 | velocity = 0; 98 | } 99 | 100 | @Override 101 | public boolean isFinished() { 102 | return false; 103 | } 104 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/NewFourBallAuto.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.auto.groups; 6 | 7 | import edu.wpi.first.math.geometry.Pose2d; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.geometry.Translation2d; 10 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 11 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 12 | import edu.wpi.first.wpilibj2.command.WaitCommand; 13 | import frc.lib.HelixJoysticks; 14 | import frc.paths.NewAutoPartFour; 15 | import frc.paths.NewAutoPartOne; 16 | import frc.paths.NewAutoPartThree; 17 | import frc.paths.NewAutoPartTwo; 18 | import frc.paths.NewFourPartOne; 19 | import frc.paths.NewFourPartThree; 20 | import frc.paths.NewFourPartTwo; 21 | import frc.robot.Robot; 22 | import frc.robot.drive.Drivetrain; 23 | import frc.robot.drive.commands.ResetOdometry; 24 | import frc.robot.drive.commands.TrajectoryFollower; 25 | import frc.robot.drive.commands.TurnToAngle; 26 | import frc.robot.intake.Intake; 27 | import frc.robot.intake.commands.FastIntake; 28 | import frc.robot.intake.commands.RetractIntake; 29 | import frc.robot.shooter.Shooter; 30 | import frc.robot.shooter.commands.FlywheelController; 31 | import frc.robot.shooter.commands.PullTrigger; 32 | import frc.robot.shooter.commands.ResetEncoder; 33 | import frc.robot.shooter.commands.StopShooter; 34 | import frc.robot.shooter.commands.StopTrigger; 35 | import frc.robot.status.actions.ImageAction; 36 | import frc.robot.status.commands.ActionCommand; 37 | import frc.robot.vision.Limelight; 38 | 39 | // NOTE: Consider using this command inline, rather than writing a subclass. For more 40 | // information, see: 41 | // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html 42 | public class NewFourBallAuto extends SequentialCommandGroup { 43 | /** Creates a new NewAuto. */ 44 | public NewFourBallAuto(Drivetrain drive, Shooter shooter, Intake intake, Limelight limelight, HelixJoysticks joysticks) { 45 | addCommands( 46 | new ResetOdometry(drive, new Pose2d(new Translation2d(0,0), new Rotation2d(-2.35))), 47 | new ResetEncoder(shooter), 48 | new ParallelDeadlineGroup( 49 | new WaitCommand(3.2), 50 | // new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()), 51 | new TrajectoryFollower(drive, new NewFourPartOne()), 52 | new FastIntake(intake), 53 | new SequentialCommandGroup( 54 | new WaitCommand(1.5), 55 | new PullTrigger(shooter, intake) 56 | ), 57 | new FlywheelController(shooter, 1840, 75)), 58 | new ParallelDeadlineGroup( 59 | new TrajectoryFollower(drive, new NewFourPartTwo()), 60 | new FastIntake(intake), 61 | new StopTrigger(shooter, intake), 62 | new StopShooter(shooter) 63 | ), 64 | new WaitCommand(0.6), 65 | new ParallelDeadlineGroup( 66 | new WaitCommand(5), 67 | new SequentialCommandGroup( 68 | new WaitCommand(1.75), 69 | new FlywheelController(shooter, 1745, 79.0) 70 | ), 71 | new SequentialCommandGroup( 72 | new TrajectoryFollower(drive, new NewFourPartThree()), 73 | new TurnToAngle(drive, limelight, joysticks) 74 | ), 75 | new SequentialCommandGroup( 76 | new WaitCommand(3.6), 77 | new PullTrigger(shooter, intake) 78 | ), 79 | new SequentialCommandGroup( 80 | new WaitCommand(0.75), 81 | new RetractIntake(intake) 82 | ) 83 | ) 84 | ); 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/ControllerPatroller.java: -------------------------------------------------------------------------------- 1 | package frc.lib; 2 | 3 | import java.util.ArrayList; 4 | import java.util.List; 5 | import java.util.Optional; 6 | 7 | import edu.wpi.first.wpilibj.Joystick; 8 | 9 | /** 10 | * This class will create joysticks for all 6 slots and allow you to retrieve them based on their display name. 11 | */ 12 | public class ControllerPatroller { 13 | 14 | private static ControllerPatroller patroller = new ControllerPatroller(); 15 | 16 | /** 17 | * Retrieve the singleton instance of the ControllerPatroller 18 | * 19 | * @return the instance of the ControllerPatroller 20 | */ 21 | public static ControllerPatroller getInstance() { 22 | if (patroller == null) { 23 | patroller = new ControllerPatroller(); 24 | } 25 | return patroller; 26 | } 27 | 28 | private List controllers = new ArrayList<>(); 29 | private List controllerNames = new ArrayList<>(); 30 | 31 | /** 32 | * @param controllerNames the list of controller names in the order the user would like to access them 33 | */ 34 | private ControllerPatroller() { 35 | // Create a joystick at each port so we can check their names later 36 | // Most of these objects will go unused 37 | for (int i = 0; i < 6; i++) { 38 | controllers.add(new Joystick(i)); 39 | controllerNames.add(controllers.get(i).getName()); 40 | } 41 | } 42 | 43 | /** 44 | * Detect if the list of controllers connected to USB has changed. 45 | * 46 | * If so, update the list of controllers and return TRUE. 47 | * 48 | * @return has the list of controllers changed? 49 | */ 50 | public boolean controllersChanged() { 51 | 52 | boolean changed = false; 53 | for (int i = 0; i < 6; i++) { 54 | String controllerName = controllers.get(i).getName(); 55 | if (!controllerNames.get(i).equals(controllerName)) { 56 | changed = true; 57 | controllerNames.set(i,controllerName); 58 | } 59 | } 60 | return changed; 61 | } 62 | 63 | /** 64 | * @param name string to look for in list of joystick names 65 | * @return Joystick or null 66 | */ 67 | public Optional find(String name) { 68 | // Filter the list of controllers for ones that contain the provided name 69 | Optional joystick = controllers.stream() 70 | .filter(c -> c.getName().toLowerCase().contains(name.toLowerCase())) 71 | .findFirst(); 72 | 73 | return joystick; 74 | } 75 | /** 76 | * @param name the name of the controller to access 77 | * @param defaultPort the port to access if a controller with the provided name is not found 78 | * 79 | * @return the joystick with the provided name or at the provided defaultPort 80 | */ 81 | public Joystick get(String name, int defaultPort) { 82 | // Filter the list of controllers for ones that contain the provided name 83 | Optional joystick = find(name); 84 | 85 | if (joystick.isPresent()) { 86 | return joystick.get(); 87 | } 88 | 89 | // If we didn't find a controller with the provided name return the one at the default port 90 | return controllers.get(defaultPort); 91 | } 92 | 93 | public Joystick get(String[] names, int defaultPort) { 94 | // Filter the list of controllers for ones that contain the provided name 95 | for (String name : names) { 96 | Optional joystick = find(name); 97 | if (joystick.isPresent()) { 98 | return joystick.get(); 99 | } 100 | } 101 | return controllers.get(defaultPort); 102 | } 103 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/FiveBallAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | import edu.wpi.first.math.geometry.Pose2d; 5 | import edu.wpi.first.math.geometry.Rotation2d; 6 | import edu.wpi.first.math.geometry.Translation2d; 7 | import edu.wpi.first.wpilibj.util.Color; 8 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 9 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 10 | import frc.paths.FiveBallPartFour; 11 | import frc.paths.FiveBallPartOne; 12 | import frc.paths.FiveBallPartThree; 13 | import frc.paths.FiveBallPartTwo; 14 | import frc.paths.Spinnnn; 15 | import frc.robot.Robot; 16 | import frc.robot.drive.Drivetrain; 17 | import frc.robot.drive.commands.ResetOdometry; 18 | import frc.robot.drive.commands.TrajectoryFollower; 19 | import frc.robot.intake.Intake; 20 | import frc.robot.intake.commands.FastIntake; 21 | import frc.robot.intake.commands.RetractIntake; 22 | import frc.robot.shooter.Shooter; 23 | import frc.robot.shooter.commands.FlywheelController; 24 | import frc.robot.shooter.commands.PullTrigger; 25 | import frc.robot.shooter.commands.ResetEncoder; 26 | import frc.robot.shooter.commands.ResetHood; 27 | import frc.robot.shooter.commands.StopShooter; 28 | import frc.robot.shooter.commands.StopTrigger; 29 | import frc.robot.status.Status; 30 | import frc.robot.status.actions.ImageAction; 31 | import frc.robot.status.commands.ActionCommand; 32 | import frc.robot.status.commands.SetColor; 33 | 34 | public class FiveBallAuto extends SequentialCommandGroup { 35 | public FiveBallAuto(Drivetrain drive, Intake intake, Shooter shooter) { 36 | addCommands( 37 | new ResetOdometry(drive, new Pose2d(new Translation2d(-0.7, 0), Rotation2d.fromDegrees(-90.0))), 38 | new ResetEncoder(shooter), 39 | new ParallelDeadlineGroup( 40 | new SequentialCommandGroup( 41 | new WaitCommand(0.8), // Give shooter time to spin up & hood to move 42 | new PullTrigger(shooter, intake), 43 | new WaitCommand(0.5)), 44 | new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()), 45 | new TrajectoryFollower(drive, new FiveBallPartOne()), // Turn to point at center 46 | new FlywheelController(shooter, 1810, 77.90)), 47 | new ParallelDeadlineGroup( 48 | new StopShooter(shooter), 49 | new StopTrigger(shooter, intake), 50 | new FastIntake(intake)), 51 | new ParallelDeadlineGroup( 52 | new WaitCommand(5.0), 53 | new SequentialCommandGroup( 54 | new WaitCommand(1.1), 55 | new FlywheelController(shooter, 1980, 73.25)), 56 | new TrajectoryFollower(drive, new FiveBallPartTwo()), 57 | new SequentialCommandGroup( 58 | new WaitCommand(3.25), 59 | new PullTrigger(shooter, intake)), 60 | new SequentialCommandGroup( 61 | new WaitCommand(4.0), 62 | new RetractIntake(intake))), 63 | new StopShooter(shooter), 64 | new StopTrigger(shooter, intake) 65 | // new ParallelDeadlineGroup( 66 | // new TrajectoryFollower(drive, new FiveBallPartThree()), 67 | // new FastIntake(intake)), 68 | // new WaitCommand(0.9), // Pick up balls 4 & 5 69 | // new ParallelDeadlineGroup( 70 | // new WaitCommand(4.5), 71 | // new SequentialCommandGroup( 72 | // new WaitCommand(1.75), 73 | // new FlywheelController(shooter, 1795, 77.60)), 74 | // new SequentialCommandGroup( 75 | // new WaitCommand(2.9), 76 | // new PullTrigger(shooter)), 77 | // new SequentialCommandGroup( 78 | // new WaitCommand(1.5), 79 | // new RetractIntake(intake)), 80 | // new TrajectoryFollower(drive, new FiveBallPartFour())), 81 | // new StopShooter(shooter), 82 | // new StopTrigger(shooter), 83 | // new ResetHood(shooter), 84 | // new SetColor(Status.getInstance(), Color.kBlack) 85 | ); 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/intake/Intake.java: -------------------------------------------------------------------------------- 1 | package frc.robot.intake; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.NeutralMode; 5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 6 | 7 | import edu.wpi.first.wpilibj.DoubleSolenoid; 8 | import edu.wpi.first.wpilibj.DoubleSolenoid.Value; 9 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 10 | import frc.robot.Constants.ElectricalConstants; 11 | import frc.robot.Constants.IntakeConstants; 12 | 13 | public class Intake extends SubsystemBase { 14 | private DoubleSolenoid solenoid = new DoubleSolenoid( 15 | ElectricalConstants.kPneumaticHub, 16 | ElectricalConstants.kIntakeDeployPort, 17 | ElectricalConstants.kIntakeRetractPort); 18 | private TalonSRX bottomMotor = new TalonSRX(ElectricalConstants.kIntakeBottomPort); 19 | private TalonSRX topMotor = new TalonSRX(ElectricalConstants.kIntakeTopPort); 20 | 21 | private boolean pushCargo = false; 22 | private boolean topRollerRunning = false; 23 | 24 | public Intake() { 25 | bottomMotor.configFactoryDefault(); 26 | bottomMotor.setNeutralMode(NeutralMode.Brake); 27 | bottomMotor.enableVoltageCompensation(true); 28 | bottomMotor.configVoltageCompSaturation(12); 29 | bottomMotor.configPeakCurrentDuration(100,0); 30 | bottomMotor.configPeakCurrentLimit(60,0); 31 | bottomMotor.configContinuousCurrentLimit(40); 32 | bottomMotor.enableCurrentLimit(true); 33 | topMotor.configFactoryDefault(); 34 | topMotor.setNeutralMode(NeutralMode.Brake); 35 | topMotor.enableVoltageCompensation(true); 36 | topMotor.configVoltageCompSaturation(12); 37 | topMotor.configPeakCurrentDuration(100,0); 38 | topMotor.configPeakCurrentLimit(60,0); 39 | topMotor.configContinuousCurrentLimit(40); 40 | topMotor.enableCurrentLimit(true); 41 | } 42 | 43 | public boolean isPushCargo() { 44 | return pushCargo; 45 | } 46 | 47 | public void setPushCargo(boolean pushCargo) { 48 | this.pushCargo = pushCargo; 49 | } 50 | 51 | /** 52 | * Decide what to do with the intake top roller. 53 | * Only run it for pushing the cargo in if the intake is in. 54 | */ 55 | public void periodic() { 56 | if (!isExtended()) { 57 | if (pushCargo) { 58 | if (!topRollerRunning) { 59 | topRollerShoot(); 60 | topRollerRunning = true; 61 | } 62 | } else if (topRollerRunning) { 63 | topRollerStop(); 64 | topRollerRunning = false; 65 | } 66 | } 67 | } 68 | 69 | public void deploy() { 70 | solenoid.set(Value.kForward); 71 | } 72 | 73 | public void retract() { 74 | solenoid.set(Value.kReverse); 75 | } 76 | 77 | public void fastRollerIn() { 78 | bottomMotor.set(ControlMode.PercentOutput, -0.85); 79 | topMotor.set(ControlMode.PercentOutput, -0.4); 80 | } 81 | 82 | // public void rollerIn() { 83 | // motor.set(ControlMode.PercentOutput, IntakeConstants.kRollerSpeed); 84 | // } 85 | 86 | public void rollerOut() { 87 | bottomMotor.set(ControlMode.PercentOutput, IntakeConstants.kRollerEjectSpeed); 88 | topMotor.set(ControlMode.PercentOutput, IntakeConstants.kRollerEjectSpeed); 89 | } 90 | 91 | public void topRollerIn() { 92 | topMotor.set(ControlMode.PercentOutput, -0.6); 93 | } 94 | 95 | public void topRollerShoot() { 96 | topMotor.set(ControlMode.PercentOutput, -0.5); 97 | } 98 | 99 | public void bottomRollerStop() { 100 | bottomMotor.set(ControlMode.PercentOutput, 0); 101 | } 102 | 103 | public void topRollerStop() { 104 | topMotor.set(ControlMode.PercentOutput, 0); 105 | } 106 | 107 | // Push a ball into the trigger 108 | // public void rollerPush() { 109 | // motor.set(ControlMode.PercentOutput, 0.30); 110 | // } 111 | 112 | // Status of the intake arm's extended state. 113 | public boolean isExtended() { 114 | return solenoid.get() == Value.kForward; 115 | // return false; 116 | } 117 | } -------------------------------------------------------------------------------- /src/main/python/trajectory_util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pylab as plt 3 | import matplotlib as mpl 4 | import json 5 | import math 6 | import os 7 | import matplotlib.animation as animation 8 | 9 | def short(theta0, theta1): 10 | diff = (theta1 - theta0 + math.pi) % (2*math.pi) - math.pi 11 | if diff < -math.pi: 12 | return diff + 2*math.pi 13 | else: 14 | return diff 15 | 16 | def solve_corners(pose, drive): 17 | x, y, theta = pose 18 | width = drive.width 19 | length = drive.length 20 | sin = np.sin(theta) 21 | cos = np.cos(theta) 22 | p0 = (x+(width/2)*cos-(length/2)*sin, y+(length/2)*cos+(width/2)*sin) 23 | p1 = (x-(width/2)*cos-(length/2)*sin, y+(length/2)*cos-(width/2)*sin) 24 | p2 = (x+(width/2)*cos+(length/2)*sin, y-(length/2)*cos+(width/2)*sin) 25 | p3 = (x-(width/2)*cos+(length/2)*sin, y-(length/2)*cos-(width/2)*sin) 26 | return [[p0, p1], [p0, p2], [p1, p3], [p2, p3]] 27 | 28 | def draw_robot(ax, pose, drive): 29 | lines = mpl.collections.LineCollection(solve_corners(pose,drive),color="black",lw=1) 30 | ax.add_collection(lines) 31 | 32 | def draw_field(): 33 | plt.style.use("classic") 34 | fig, ax = plt.subplots() 35 | ax.add_patch(mpl.patches.Rectangle( 36 | (0, 0), 37 | 30, 38 | 15, 39 | lw=4, 40 | edgecolor="black", 41 | facecolor="none", 42 | )) 43 | plt.title("Trajectory") 44 | plt.xlabel("X Position (meters)") 45 | plt.ylabel("y Position (meters)") 46 | plt.ylim(0,8.23) 47 | plt.xlim(0,16.46) 48 | plt.gca().set_aspect("equal", adjustable="box") 49 | return fig, ax 50 | 51 | def draw_trajectory(x_coords, y_coords, angular_coords, waypoints, drive, title): 52 | fig, ax = draw_field() 53 | draw_robot(ax,[x_coords[0],y_coords[0],angular_coords[0]],drive) 54 | draw_robot(ax,[x_coords[-1],y_coords[-1],angular_coords[-1]],drive) 55 | plt.plot(x_coords,y_coords,color="b") 56 | plt.title(title) 57 | for waypoint in waypoints: 58 | draw_robot(ax, waypoint, drive) 59 | # for pose in zip(x_coords, y_coords, angular_coords): 60 | # draw_robot(ax,pose, drive) 61 | 62 | def animate_trajectory( 63 | x_coords, 64 | y_coords, 65 | angular_coords, 66 | waypoints, 67 | drive, 68 | dt, 69 | title 70 | ): 71 | 72 | fig, ax = draw_field() 73 | 74 | for waypoint in waypoints: 75 | draw_robot(ax, waypoint, drive) 76 | 77 | num_states = len(x_coords) 78 | plt.plot(x_coords, y_coords) 79 | 80 | def animate(i): 81 | pose = list(zip(x_coords, y_coords, angular_coords))[i] 82 | ax.collections = ax.collections[:7] 83 | 84 | draw_robot(ax, pose, drive) 85 | return ax.collections 86 | 87 | anim = animation.FuncAnimation( 88 | fig, animate, frames=num_states, interval=dt, blit=True, repeat=True 89 | ) 90 | 91 | if not os.path.exists("animations"): 92 | os.makedirs("animations") 93 | anim.save( 94 | os.path.join("animations", "{}.gif".format(title)), 95 | writer="pillow", 96 | dpi=100, 97 | fps=(int)(1 / dt), 98 | ) 99 | return anim 100 | 101 | def generate_initial_trajectory(waypoints, num_states): 102 | x, y, theta = [], [], [] 103 | 104 | lengths = [0] 105 | for k in range(len(waypoints)-1): 106 | lengths.append(lengths[k] + math.hypot(waypoints[k+1][0]-waypoints[k][0],waypoints[k+1][1]-waypoints[k][1])) 107 | ds = lengths[-1] / (num_states - 1) 108 | 109 | index = 0 110 | for k in range(num_states): 111 | s = ds * k 112 | while (lengths[index + 1] + 0.000001 < s): 113 | index += 1 114 | t = (s - lengths[index]) / (lengths[index + 1] - lengths[index]) 115 | x.append((waypoints[index + 1][0] - waypoints[index][0]) * t + waypoints[index][0]) 116 | y.append((waypoints[index + 1][1] - waypoints[index][1]) * t + waypoints[index][1]) 117 | theta.append((waypoints[index + 1][2] - waypoints[index][2]) * t + waypoints[index][2]) 118 | return x, y, theta -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/FourBallAuto.java: -------------------------------------------------------------------------------- 1 | package frc.robot.auto.groups; 2 | 3 | import edu.wpi.first.wpilibj2.command.WaitCommand; 4 | import edu.wpi.first.math.geometry.Pose2d; 5 | import edu.wpi.first.math.geometry.Rotation2d; 6 | import edu.wpi.first.math.geometry.Translation2d; 7 | import edu.wpi.first.wpilibj.util.Color; 8 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 9 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 10 | import frc.lib.HelixJoysticks; 11 | import frc.paths.CollectSecondBall; 12 | import frc.paths.FourBallPartThree; 13 | import frc.paths.FourBallPartTwo; 14 | import frc.paths.GoHome; 15 | import frc.paths.OnePointEightMetersForward; 16 | import frc.paths.WeirdAutoPartOne; 17 | import frc.robot.Robot; 18 | import frc.robot.drive.Drivetrain; 19 | import frc.robot.drive.commands.ResetOdometry; 20 | import frc.robot.drive.commands.TrajectoryFollower; 21 | import frc.robot.drive.commands.TurnToAngle; 22 | import frc.robot.intake.Intake; 23 | import frc.robot.intake.commands.FastIntake; 24 | import frc.robot.intake.commands.RetractIntake; 25 | import frc.robot.shooter.Shooter; 26 | import frc.robot.shooter.commands.FlywheelController; 27 | import frc.robot.shooter.commands.PullTrigger; 28 | import frc.robot.shooter.commands.ResetEncoder; 29 | import frc.robot.shooter.commands.ResetHood; 30 | import frc.robot.shooter.commands.StopShooter; 31 | import frc.robot.shooter.commands.StopTrigger; 32 | import frc.robot.status.Status; 33 | import frc.robot.status.actions.ImageAction; 34 | import frc.robot.status.commands.ActionCommand; 35 | import frc.robot.status.commands.SetColor; 36 | import frc.robot.vision.Limelight; 37 | 38 | public class FourBallAuto extends SequentialCommandGroup{ 39 | 40 | public FourBallAuto(Drivetrain drive, Intake intake, Shooter shooter, Limelight limelight, HelixJoysticks joysticks) { 41 | addCommands( 42 | new ActionCommand(new ImageAction(Robot.fourBallAutoImage,0.02, ImageAction.FOREVER).oscillate().brightness(0.7)), 43 | new ResetOdometry(drive, new Pose2d(new Translation2d(0,0),Rotation2d.fromDegrees(-90))), 44 | new ResetEncoder(shooter), 45 | new ParallelDeadlineGroup( 46 | new WaitCommand(4.25), 47 | // new ResetHood(shooter), 48 | new SequentialCommandGroup( 49 | new WaitCommand(1.75), 50 | new FlywheelController(shooter, 1740, 78.25)), 51 | new SequentialCommandGroup( 52 | new WaitCommand(3.25), 53 | new PullTrigger(shooter, intake)), 54 | new TrajectoryFollower(drive, new WeirdAutoPartOne()), 55 | new SequentialCommandGroup( 56 | new ParallelDeadlineGroup( 57 | new WaitCommand(1.95), 58 | new FastIntake(intake)), 59 | new RetractIntake(intake))), 60 | new StopTrigger(shooter, intake), 61 | new StopShooter(shooter), 62 | new ParallelDeadlineGroup( 63 | new TrajectoryFollower(drive, new FourBallPartTwo()), 64 | new FastIntake(intake)), 65 | new WaitCommand(0.5), 66 | new ParallelDeadlineGroup( 67 | new WaitCommand(4.75), 68 | new SequentialCommandGroup( 69 | new WaitCommand(1.75), 70 | new FlywheelController(shooter, 1750, 79.5)), 71 | new SequentialCommandGroup( 72 | new WaitCommand(0.75), 73 | new RetractIntake(intake)), 74 | new SequentialCommandGroup( 75 | new WaitCommand(3.35), 76 | new PullTrigger(shooter, intake)), 77 | new SequentialCommandGroup( 78 | new TrajectoryFollower(drive, new FourBallPartThree()), 79 | new TurnToAngle(drive, limelight, joysticks) 80 | )), 81 | new StopShooter(shooter), 82 | new StopTrigger(shooter, intake), 83 | new ResetHood(shooter), 84 | new SetColor(Status.getInstance(), Color.kBlack) 85 | ); 86 | } 87 | } -------------------------------------------------------------------------------- /src/main/python/trajectory_generator.py: -------------------------------------------------------------------------------- 1 | from casadi import * 2 | import pylab as plt 3 | 4 | from trajectory_io import * 5 | import trajectory_util 6 | 7 | class trajectory_generator: 8 | def __init__(self, drive): 9 | self.drive = drive 10 | 11 | def generate(self, waypoints, name): 12 | self.N_per_segment = 100 13 | self.segments = len(waypoints) - 1 14 | self.N = self.N_per_segment * self.segments 15 | 16 | self.opti = Opti() 17 | 18 | # Minimize time 19 | Ts, dts = [], [] 20 | for k in range(self.segments): 21 | T = self.opti.variable() 22 | dt = T / self.N_per_segment 23 | Ts.append(T) 24 | dts.append(dt) 25 | 26 | self.opti.subject_to(T >= 0) 27 | self.opti.set_initial(T, 5) 28 | self.opti.minimize(sum(Ts)) 29 | 30 | # Initialize variables 31 | self.X = self.opti.variable(6, self.N+1) 32 | self.x = self.X[0,:] 33 | self.y = self.X[1,:] 34 | self.theta = self.X[2,:] 35 | self.vx = self.X[3,:] 36 | self.vy = self.X[4,:] 37 | self.omega = self.X[5,:] 38 | self.U = self.opti.variable(3, self.N) 39 | self.ax = self.U[0,:] 40 | self.ay = self.U[1,:] 41 | self.alpha = self.U[2,:] 42 | 43 | # Add dynamics constraint 44 | dynamics = lambda x, u: vertcat( 45 | x[3], 46 | x[4], 47 | x[5], 48 | u[0], 49 | u[1], 50 | u[2] 51 | ) 52 | 53 | for k in range(self.N): 54 | x_next = self.X[:, k] + dynamics(self.X[:, k], self.U[:, k]) * dts[int(k / self.N_per_segment)] 55 | self.opti.subject_to(self.X[:, k + 1] == x_next) 56 | 57 | # Dynamics constraint 58 | for k in range(self.N): 59 | # RK4 integration 60 | h = dts[int(k / vars_per_segment)] 61 | x_k = X[:, k] 62 | u_k = U[:, k] 63 | x_k1 = X[:, k + 1] 64 | 65 | k1 = self.f(x_k, u_k) 66 | k2 = self.f(x_k + h / 2 * k1, u_k) 67 | k3 = self.f(x_k + h / 2 * k2, u_k) 68 | k4 = self.f(x_k + h * k3, u_k) 69 | self.opti.subject_to(x_k1 == x_k + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)) 70 | 71 | # Set initial guess 72 | x_init, y_init, theta_init = trajectory_util.generate_initial_trajectory( 73 | waypoints, self.N+1 74 | ) 75 | self.opti.set_initial(self.x, x_init) 76 | self.opti.set_initial(self.y, y_init) 77 | self.opti.set_initial(self.theta, theta_init) 78 | 79 | # Add constraints 80 | self.drive.add_kinematics_constraint(self.opti, self.theta, self.vx, self.vy, self.omega, self.ax, self.ay, self.alpha, self.N) 81 | self.add_boundry_constraint() 82 | self.add_waypoint_constraint(waypoints) 83 | 84 | self.opti.solver("ipopt") 85 | sol = self.opti.solve() 86 | 87 | sol_dts = [] 88 | for k in range(self.segments): 89 | sol_dts.append(sol.value(Ts[k] / self.N_per_segment)) 90 | print(sum(sol_dts) * self.N_per_segment) 91 | 92 | xs, ys, thetas = export_trajectory(sol.value(self.x), 93 | sol.value(self.y), 94 | sol.value(self.theta), 95 | sol.value(self.vx), 96 | sol.value(self.vy), 97 | sol.value(self.omega), 98 | sol_dts, self.N_per_segment, name) 99 | 100 | trajectory_util.draw_trajectory(xs,ys,thetas,waypoints,self.drive,name) 101 | # trajectory_util.animate_trajectory(xs,ys,thetas,waypoints,self.drive,0.02,"trajectory") 102 | 103 | plt.show() 104 | 105 | def add_boundry_constraint(self): 106 | for k in [-1, 0]: 107 | self.opti.subject_to(self.vx[k] == 0) 108 | self.opti.subject_to(self.vy[k] == 0) 109 | self.opti.subject_to(self.omega[k] == 0) 110 | 111 | def add_waypoint_constraint(self, waypoints): 112 | for k in range(self.segments + 1): 113 | index = k * self.N_per_segment 114 | self.opti.subject_to(self.x[index] == waypoints[k][0]) 115 | self.opti.subject_to(self.y[index] == waypoints[k][1]) 116 | self.opti.subject_to(self.theta[index] == waypoints[k][2]) 117 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/auto/groups/NewFiveBallAuto.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.auto.groups; 6 | 7 | import edu.wpi.first.math.geometry.Pose2d; 8 | import edu.wpi.first.math.geometry.Rotation2d; 9 | import edu.wpi.first.math.geometry.Translation2d; 10 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; 11 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 12 | import edu.wpi.first.wpilibj2.command.WaitCommand; 13 | import frc.lib.HelixJoysticks; 14 | import frc.paths.NewAutoPartFour; 15 | import frc.paths.NewAutoPartOne; 16 | import frc.paths.NewAutoPartThree; 17 | import frc.paths.NewAutoPartTwo; 18 | import frc.robot.Robot; 19 | import frc.robot.drive.Drivetrain; 20 | import frc.robot.drive.commands.ResetOdometry; 21 | import frc.robot.drive.commands.TrajectoryFollower; 22 | import frc.robot.drive.commands.TurnToAngle; 23 | import frc.robot.intake.Intake; 24 | import frc.robot.intake.commands.FastIntake; 25 | import frc.robot.intake.commands.RetractIntake; 26 | import frc.robot.shooter.Shooter; 27 | import frc.robot.shooter.commands.FlywheelController; 28 | import frc.robot.shooter.commands.PullTrigger; 29 | import frc.robot.shooter.commands.ResetEncoder; 30 | import frc.robot.shooter.commands.StopShooter; 31 | import frc.robot.shooter.commands.StopTrigger; 32 | import frc.robot.status.actions.ImageAction; 33 | import frc.robot.status.commands.ActionCommand; 34 | import frc.robot.vision.Limelight; 35 | 36 | // NOTE: Consider using this command inline, rather than writing a subclass. For more 37 | // information, see: 38 | // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html 39 | public class NewFiveBallAuto extends SequentialCommandGroup { 40 | /** Creates a new NewAuto. */ 41 | public NewFiveBallAuto(Drivetrain drive, Shooter shooter, Intake intake, Limelight limelight, HelixJoysticks joysticks) { 42 | addCommands( 43 | new ResetOdometry(drive, new Pose2d(new Translation2d(0,0), new Rotation2d(-2.35))), 44 | new ResetEncoder(shooter), 45 | new ParallelDeadlineGroup( 46 | new WaitCommand(2.9), 47 | new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()), 48 | new TrajectoryFollower(drive, new NewAutoPartOne()), 49 | new FastIntake(intake), 50 | new SequentialCommandGroup( 51 | new WaitCommand(1.25), 52 | new PullTrigger(shooter, intake) 53 | ), 54 | new FlywheelController(shooter, 1840, 75)), 55 | new ParallelDeadlineGroup( 56 | new TrajectoryFollower(drive, new NewAutoPartTwo()), 57 | new FastIntake(intake), 58 | new StopTrigger(shooter, intake), 59 | new StopShooter(shooter) 60 | ), 61 | new WaitCommand(0.35), 62 | new ParallelDeadlineGroup( 63 | new WaitCommand(4.3), 64 | new SequentialCommandGroup( 65 | new WaitCommand(1.75), 66 | new FlywheelController(shooter, 1730, 81.0) 67 | ), 68 | new SequentialCommandGroup( 69 | new TrajectoryFollower(drive, new NewAutoPartThree()), 70 | new TurnToAngle(drive, limelight, joysticks) 71 | ), 72 | new SequentialCommandGroup( 73 | new WaitCommand(3.1), 74 | new PullTrigger(shooter, intake) 75 | ), 76 | new SequentialCommandGroup( 77 | new WaitCommand(0.75), 78 | new RetractIntake(intake) 79 | ) 80 | ), 81 | new ParallelDeadlineGroup( 82 | new WaitCommand(3.0), 83 | new TrajectoryFollower(drive, new NewAutoPartFour()), 84 | new FlywheelController(shooter, 1710, 80), 85 | new SequentialCommandGroup( 86 | new StopTrigger(shooter, intake), 87 | new WaitCommand(1.25), 88 | new PullTrigger(shooter, intake) 89 | ), 90 | new FastIntake(intake) 91 | ), 92 | new ParallelDeadlineGroup( 93 | new WaitCommand(1.0), 94 | new StopTrigger(shooter, intake), 95 | new StopShooter(shooter), 96 | new RetractIntake(intake)) 97 | ); 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /src/main/java/frc/paths/FiveBallPartOne.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class FiveBallPartOne extends Path { 6 | private final static double[][] points = { 7 | {0,0.0,0.0,-1.57,-0.0,-0.0,0.0}, 8 | {0.0025,-0.0,-0.0,-1.57,-0.0,0.0,-0.0446}, 9 | {0.005,-0.0,0.0,-1.5701,-0.0,0.0,-0.0892}, 10 | {0.0075,-0.0,0.0,-1.5703,-0.0,0.0,-0.1338}, 11 | {0.01,-0.0,0.0,-1.5707,-0.0,0.0,-0.1784}, 12 | {0.0126,-0.0,0.0,-1.5711,-0.0,0.0,-0.223}, 13 | {0.0151,-0.0,0.0,-1.5717,-0.0,0.0,-0.2675}, 14 | {0.0176,-0.0,0.0,-1.5724,-0.0,0.0,-0.3121}, 15 | {0.0201,-0.0,0.0,-1.5731,-0.0,0.0,-0.3567}, 16 | {0.0226,-0.0,0.0,-1.574,-0.0,0.0,-0.4013}, 17 | {0.0251,-0.0,0.0,-1.575,-0.0,0.0,-0.4459}, 18 | {0.0276,-0.0,0.0,-1.5762,-0.0,0.0,-0.4905}, 19 | {0.0301,-0.0,0.0,-1.5774,-0.0,0.0,-0.5351}, 20 | {0.0327,-0.0,0.0,-1.5787,-0.0,0.0,-0.5797}, 21 | {0.0352,-0.0,0.0,-1.5802,-0.0,0.0,-0.6243}, 22 | {0.0377,-0.0,0.0,-1.5818,-0.0,0.0,-0.6689}, 23 | {0.0402,-0.0,0.0,-1.5834,-0.0,0.0,-0.7134}, 24 | {0.0427,-0.0,0.0,-1.5852,-0.0,0.0,-0.758}, 25 | {0.0452,-0.0,0.0,-1.5871,-0.0,0.0,-0.8026}, 26 | {0.0477,-0.0,0.0,-1.5892,-0.0,0.0,-0.8472}, 27 | {0.0502,-0.0,0.0,-1.5913,-0.0,0.0,-0.8918}, 28 | {0.0527,-0.0,0.0,-1.5935,-0.0,0.0,-0.9364}, 29 | {0.0553,-0.0,0.0,-1.5959,-0.0,0.0,-0.981}, 30 | {0.0578,-0.0,0.0,-1.5983,-0.0,0.0,-1.0256}, 31 | {0.0603,-0.0,0.0,-1.6009,-0.0,0.0,-1.0702}, 32 | {0.0628,-0.0,0.0,-1.6036,-0.0,0.0,-1.1148}, 33 | {0.0653,-0.0,0.0,-1.6064,-0.0,0.0,-1.1593}, 34 | {0.0678,-0.0,0.0,-1.6093,-0.0,0.0,-1.2039}, 35 | {0.0703,-0.0,0.0,-1.6123,-0.0,0.0,-1.2485}, 36 | {0.0728,-0.0,0.0,-1.6155,-0.0,0.0,-1.2931}, 37 | {0.0754,-0.0,0.0,-1.6187,-0.0,0.0,-1.3377}, 38 | {0.0779,-0.0,0.0,-1.6221,-0.0,0.0,-1.3823}, 39 | {0.0804,-0.0,0.0,-1.6256,-0.0,0.0,-1.4269}, 40 | {0.0829,-0.0,0.0,-1.6291,-0.0,0.0,-1.4715}, 41 | {0.0854,-0.0,0.0,-1.6328,-0.0,0.0,-1.5161}, 42 | {0.0879,-0.0,0.0,-1.6366,-0.0,0.0,-1.5607}, 43 | {0.0904,-0.0,0.0,-1.6406,-0.0,0.0,-1.6052}, 44 | {0.0929,-0.0,0.0,-1.6446,-0.0,0.0,-1.6498}, 45 | {0.0954,-0.0,0.0,-1.6487,-0.0,0.0,-1.6944}, 46 | {0.098,-0.0,0.0,-1.653,-0.0,0.0,-1.739}, 47 | {0.1005,-0.0,0.0,-1.6574,-0.0,0.0,-1.7836}, 48 | {0.103,-0.0,0.0,-1.6618,-0.0,0.0,-1.8282}, 49 | {0.1055,-0.0,0.0,-1.6664,-0.0,0.0,-1.8728}, 50 | {0.108,-0.0,0.0,-1.6711,-0.0,0.0,-1.9174}, 51 | {0.1105,-0.0,0.0,-1.676,-0.0,0.0,-1.962}, 52 | {0.113,-0.0,0.0,-1.6809,-0.0,0.0,-2.0066}, 53 | {0.1155,-0.0,0.0,-1.6859,-0.0,0.0,-2.0511}, 54 | {0.1181,-0.0,0.0,-1.6911,-0.0,0.0,-2.0957}, 55 | {0.1206,-0.0,0.0,-1.6963,-0.0,0.0,-2.1403}, 56 | {0.1231,-0.0,0.0,-1.7017,-0.0,0.0,-2.1849}, 57 | {0.1256,-0.0,0.0,-1.7072,0.0,0.0,-2.2295}, 58 | {0.1281,-0.0,0.0,-1.7128,0.0,0.0,-2.1849}, 59 | {0.1306,-0.0,0.0,-1.7183,0.0,0.0,-2.1403}, 60 | {0.1331,-0.0,0.0,-1.7237,0.0,0.0,-2.0957}, 61 | {0.1356,-0.0,0.0,-1.7289,0.0,0.0,-2.0511}, 62 | {0.1381,-0.0,0.0,-1.7341,0.0,0.0,-2.0066}, 63 | {0.1407,-0.0,0.0,-1.7391,0.0,0.0,-1.962}, 64 | {0.1432,-0.0,0.0,-1.744,0.0,0.0,-1.9174}, 65 | {0.1457,-0.0,0.0,-1.7489,0.0,0.0,-1.8728}, 66 | {0.1482,-0.0,0.0,-1.7536,0.0,0.0,-1.8282}, 67 | {0.1507,-0.0,0.0,-1.7582,0.0,0.0,-1.7836}, 68 | {0.1532,-0.0,0.0,-1.7626,0.0,0.0,-1.739}, 69 | {0.1557,-0.0,0.0,-1.767,0.0,0.0,-1.6944}, 70 | {0.1582,-0.0,0.0,-1.7713,0.0,0.0,-1.6498}, 71 | {0.1608,-0.0,0.0,-1.7754,0.0,0.0,-1.6052}, 72 | {0.1633,-0.0,0.0,-1.7794,0.0,0.0,-1.5607}, 73 | {0.1658,-0.0,0.0,-1.7834,0.0,0.0,-1.5161}, 74 | {0.1683,-0.0,0.0,-1.7872,0.0,0.0,-1.4715}, 75 | {0.1708,-0.0,0.0,-1.7909,0.0,0.0,-1.4269}, 76 | {0.1733,-0.0,0.0,-1.7944,0.0,0.0,-1.3823}, 77 | {0.1758,-0.0,0.0,-1.7979,0.0,0.0,-1.3377}, 78 | {0.1783,-0.0,0.0,-1.8013,0.0,0.0,-1.2931}, 79 | {0.1808,-0.0,0.0,-1.8045,0.0,0.0,-1.2485}, 80 | {0.1834,-0.0,0.0,-1.8077,0.0,0.0,-1.2039}, 81 | {0.1859,-0.0,0.0,-1.8107,0.0,0.0,-1.1593}, 82 | {0.1884,-0.0,0.0,-1.8136,0.0,0.0,-1.1148}, 83 | {0.1909,-0.0,0.0,-1.8164,0.0,0.0,-1.0702}, 84 | {0.1934,-0.0,0.0,-1.8191,0.0,0.0,-1.0256}, 85 | {0.1959,-0.0,0.0,-1.8217,0.0,0.0,-0.981}, 86 | {0.1984,-0.0,0.0,-1.8241,0.0,0.0,-0.9364}, 87 | {0.2009,-0.0,0.0,-1.8265,0.0,0.0,-0.8918}, 88 | {0.2035,-0.0,0.0,-1.8287,0.0,0.0,-0.8472}, 89 | {0.206,-0.0,0.0,-1.8308,0.0,0.0,-0.8026}, 90 | {0.2085,-0.0,0.0,-1.8329,0.0,0.0,-0.758}, 91 | {0.211,-0.0,0.0,-1.8348,0.0,0.0,-0.7134}, 92 | {0.2135,-0.0,0.0,-1.8366,0.0,0.0,-0.6689}, 93 | {0.216,-0.0,0.0,-1.8382,0.0,0.0,-0.6243}, 94 | {0.2185,-0.0,0.0,-1.8398,0.0,0.0,-0.5797}, 95 | {0.221,-0.0,0.0,-1.8413,0.0,0.0,-0.5351}, 96 | {0.2235,-0.0,0.0,-1.8426,0.0,0.0,-0.4905}, 97 | {0.2261,-0.0,0.0,-1.8438,0.0,0.0,-0.4459}, 98 | {0.2286,-0.0,0.0,-1.845,0.0,0.0,-0.4013}, 99 | {0.2311,-0.0,0.0,-1.846,0.0,0.0,-0.3567}, 100 | {0.2336,-0.0,0.0,-1.8469,0.0,0.0,-0.3121}, 101 | {0.2361,-0.0,0.0,-1.8476,0.0,0.0,-0.2675}, 102 | {0.2386,-0.0,0.0,-1.8483,0.0,0.0,-0.223}, 103 | {0.2411,-0.0,0.0,-1.8489,0.0,0.0,-0.1784}, 104 | {0.2436,-0.0,0.0,-1.8493,0.0,0.0,-0.1338}, 105 | {0.2462,-0.0,0.0,-1.8497,0.0,0.0,-0.0892}, 106 | {0.2487,-0.0,0.0,-1.8499,0.0,0.0,-0.0446}, 107 | {0.2512,0.0,0.0,-1.85,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/python/paths.py: -------------------------------------------------------------------------------- 1 | from trajectory_generator import trajectory_generator 2 | from drive.swerve_drive import swerve_drive 3 | import trajectory_io 4 | import math 5 | 6 | def main(): 7 | drive = swerve_drive( 8 | # Wheelbase x/y 9 | 0.622,0.572, 10 | # Bumper length/width 11 | 0.954,0.903, 12 | # Mass/moi 13 | 46.7,5.6, 14 | # Max velocity/force 15 | 70, 1.9, 16 | # 73, 1, 17 | # 50, 1, 18 | # Wheel radius 19 | 0.051) 20 | 21 | generator = trajectory_generator(drive) 22 | 23 | # generator.generate( 24 | # [[0,0,-1.57], 25 | # [0,1.0,-1.57], 26 | # [0,0,-1.95]], 27 | # "WeirdAutoPartOne") 28 | 29 | # generator.generate( 30 | # [[0,0,-1.95], 31 | # [-1.25,1.225,-0.6]], 32 | # "WeirdAutoPartTwo") 33 | 34 | # generator.generate( 35 | # [[-1.0,0,-0.15], 36 | # [0,2,-math.pi/2], 37 | # [-0.42,6.25,-1]], 38 | # "CollectSecondBall") 39 | 40 | # generator.generate( 41 | # [[-0.42,6.121,-1], 42 | # [0.0,0.5,-0.59]], 43 | # "GoHome") 44 | 45 | # generator.generate([ 46 | # [0,0,2.32], 47 | # [1.2,-0.15,2.62], 48 | # [0.35,-0.35,2.32]], 49 | # "TwoBallPartOne") 50 | 51 | # generator.generate([ 52 | # [0.35,-0.35,2.32], 53 | # [1.8,-1.52,0], 54 | # [0.9,-1.52,0]], 55 | # "TwoBallPartTwo") 56 | 57 | # generator.generate([ 58 | # [0.35,-0.35,2.32], 59 | # [0.5,-1.2,math.pi/2], 60 | # [0.85,2,math.pi], 61 | # [2,2.3,math.pi], 62 | # [3.6,2.3,6.45/4*math.pi]], 63 | # "SuperRude") 64 | 65 | # generator.generate([ 66 | # [0.35,-0.35,2.32], 67 | # [0.7,-1.2,math.pi/2], 68 | # # [2.5,0.5,3/2*math.pi], 69 | # [2.8,1.45,3.4/2*math.pi], 70 | # [2.4,2,3.4/2*math.pi], 71 | # [3.4,2,3.4/2*math.pi]], 72 | # "SuperRudeTwo") 73 | 74 | # generator.generate([ 75 | # [0,0,-1.95], 76 | # [2.5,0.85,-1.95], 77 | # [5.7,-0.3,-2.1], 78 | # [6.35,0.0,-2.1]], 79 | # "FourBallPartTwo") 80 | 81 | generator.generate([ 82 | [0,0,-1.95], 83 | [2.5,0.85,-1.95], 84 | [6,-0.3,-2.1], 85 | [6.3,0,-2.3], 86 | [6,-0.3,-2.3]], 87 | "FourBallPartTwo") 88 | 89 | generator.generate([ 90 | [6,-0.3,-2.3], 91 | [2.5,0.85,-2.1], 92 | [-0.3,0,-1.95]], 93 | "FourBallPartThree") 94 | 95 | # generator.generate([ 96 | # [6.15,0.15,-2.3], 97 | # [2.5,0.85,-2.1], 98 | # [0.2,-0.3,-2.0]], 99 | # "FourBallPartThree") 100 | 101 | # generator.generate([ 102 | # [0,0,-1.57], 103 | # [0,0.00000001,-1.85]], 104 | # "FiveBallPartOne") 105 | 106 | # generator.generate([ 107 | # [-0.7,0,-math.pi/2], 108 | # [-0.2,0.9,-math.pi/2+0.15], 109 | # [1.85,-0.5,-math.pi+0.6] 110 | # ], 111 | # "FiveBallPartTwo") 112 | 113 | # generator.generate([ 114 | # [1.85,-0.5,-math.pi+0.6], 115 | # [6.15,0.3,-math.pi+0.7] 116 | # ], 117 | # "FiveBallPartThree") 118 | 119 | # generator.generate([ 120 | # [6.15,0.3,-math.pi+0.7], 121 | # [0.10,-0.375,-math.pi+1.25] 122 | # ], 123 | # "FiveBallPartFour") 124 | 125 | # generator.generate([ 126 | # [-0,0,-2.35], 127 | # [1.1,0.3,-2.55], 128 | # [0.95,0.15,-2.55], 129 | # ], 130 | # "NewAutoPartOne") 131 | 132 | # generator.generate([ 133 | # [0.95,0.15,-2.55], 134 | # [5.0,0.5,-2.275], 135 | # [5.2,0.75,-2.275], 136 | # [4.85,0.4,-2.275] 137 | # ], 138 | # "NewAutoPartTwo") 139 | 140 | # generator.generate([ 141 | # [4.85,0.4,-2.275], 142 | # [-1.3,0.775,-math.pi/2-0.2] 143 | # ], 144 | # "NewAutoPartThree") 145 | 146 | # generator.generate([ 147 | # [-1.3,0.775,-math.pi/2-0.2], 148 | # [-1.1,1.7,-math.pi/2-0.14], 149 | # [-1.3,0.9,-math.pi/2-0.2] 150 | # ], 151 | # "NewAutoPartFour") 152 | 153 | # generator.generate([ 154 | # [-0,0,-2.35], 155 | # [1.1,0.3,-2.55], 156 | # [0.95,0.15,-2.55], 157 | # ], 158 | # "NewFourPartOne") 159 | 160 | # generator.generate([ 161 | # [0.95,0.15,-2.55], 162 | # [5.0,0.5,-2.275], 163 | # [5.2,0.75,-2.275], 164 | # [4.85,0.4,-2.275] 165 | # ], 166 | # "NewFourPartTwo") 167 | 168 | # generator.generate([ 169 | # [4.85,0.4,-2.275], 170 | # [0.2,0.2,-2.35] 171 | # ], 172 | # "NewFourPartThree") 173 | 174 | main() -------------------------------------------------------------------------------- /src/main/java/frc/paths/ShootTwoBalls.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class ShootTwoBalls extends Path { 6 | private final static double[][] points = { 7 | {0,0.0,0.0,2.32,-0.0,0.0,0.0}, 8 | {0.0138,-0.0,0.0,2.32,0.0348,-0.0,0.0085}, 9 | {0.0276,0.0005,-0.0,2.3201,0.0695,-0.0,0.017}, 10 | {0.0414,0.0014,-0.0,2.3204,0.1043,-0.0,0.0256}, 11 | {0.0552,0.0029,-0.0,2.3207,0.139,-0.0,0.0341}, 12 | {0.0691,0.0048,-0.0,2.3212,0.1738,-0.0,0.0426}, 13 | {0.0829,0.0072,-0.0,2.3218,0.2085,-0.0,0.0511}, 14 | {0.0967,0.0101,-0.0,2.3225,0.2433,-0.0,0.0597}, 15 | {0.1105,0.0134,-0.0,2.3233,0.278,-0.0,0.0682}, 16 | {0.1243,0.0173,-0.0,2.3242,0.3128,-0.0,0.0767}, 17 | {0.1381,0.0216,-0.0,2.3253,0.3475,-0.0,0.0852}, 18 | {0.1519,0.0264,-0.0,2.3265,0.3823,-0.0,0.0937}, 19 | {0.1657,0.0317,-0.0,2.3278,0.417,-0.0,0.1023}, 20 | {0.1796,0.0374,-0.0,2.3292,0.4518,-0.0,0.1108}, 21 | {0.1934,0.0437,-0.0,2.3307,0.4865,-0.0,0.1193}, 22 | {0.2072,0.0504,-0.0,2.3324,0.5213,-0.0,0.1278}, 23 | {0.221,0.0576,-0.0,2.3341,0.556,-0.0,0.1364}, 24 | {0.2348,0.0653,-0.0,2.336,0.5908,-0.0,0.1449}, 25 | {0.2486,0.0734,-0.0,2.338,0.6256,-0.0,0.1534}, 26 | {0.2624,0.0821,-0.0,2.3401,0.6603,-0.0,0.162}, 27 | {0.2762,0.0912,-0.0,2.3424,0.6951,-0.0,0.1705}, 28 | {0.2901,0.1008,-0.0,2.3447,0.7298,-0.0,0.179}, 29 | {0.3039,0.1109,-0.0,2.3472,0.7646,-0.0,0.1876}, 30 | {0.3177,0.1214,-0.0,2.3498,0.7993,-0.0,0.1961}, 31 | {0.3315,0.1325,-0.0,2.3525,0.8341,-0.0,0.2047}, 32 | {0.3453,0.144,-0.0,2.3553,0.8688,-0.0,0.2132}, 33 | {0.3591,0.156,-0.0,2.3583,0.9036,-0.0,0.2218}, 34 | {0.3729,0.1685,-0.0,2.3613,0.9383,-0.0,0.2304}, 35 | {0.3867,0.1814,-0.0,2.3645,0.9731,-0.0,0.2389}, 36 | {0.4005,0.1949,-0.0,2.3678,1.0078,-0.0,0.2475}, 37 | {0.4144,0.2088,-0.0,2.3712,1.0426,-0.0,0.2561}, 38 | {0.4282,0.2232,-0.0,2.3748,1.0773,-0.0,0.2647}, 39 | {0.442,0.2381,-0.0,2.3784,1.1121,-0.0,0.2732}, 40 | {0.4558,0.2534,-0.0,2.3822,1.1468,-0.0,0.2818}, 41 | {0.4696,0.2693,-0.0,2.3861,1.1816,-0.0,0.2904}, 42 | {0.4834,0.2856,-0.0,2.3901,1.2164,-0.0,0.299}, 43 | {0.4972,0.3024,-0.0,2.3942,1.2511,-0.0,0.3077}, 44 | {0.511,0.3197,-0.0,2.3985,1.2859,-0.0,0.3163}, 45 | {0.5249,0.3374,-0.0,2.4028,1.3206,-0.0,0.3249}, 46 | {0.5387,0.3557,-0.0,2.4073,1.3554,-0.0,0.3336}, 47 | {0.5525,0.3744,-0.0,2.4119,1.3901,-0.0,0.3422}, 48 | {0.5663,0.3936,-0.0,2.4167,1.4249,-0.0,0.3509}, 49 | {0.5801,0.4133,-0.0,2.4215,1.4596,-0.0,0.3595}, 50 | {0.5939,0.4334,-0.0,2.4265,1.4944,-0.0,0.3682}, 51 | {0.6077,0.4541,-0.0,2.4316,1.5291,-0.0,0.3769}, 52 | {0.6215,0.4752,-0.0,2.4368,1.5639,-0.0,0.3856}, 53 | {0.6354,0.4968,-0.0,2.4421,1.5986,-0.0,0.3944}, 54 | {0.6492,0.5189,-0.0,2.4475,1.6334,-0.0,0.4032}, 55 | {0.663,0.5414,-0.0,2.4531,1.6681,-0.0,0.412}, 56 | {0.6768,0.5645,-0.0,2.4588,1.7029,-0.0,0.4209}, 57 | {0.6906,0.588,-0.0,2.4646,1.7376,-0.0,0.43}, 58 | {0.7044,0.612,-0.0,2.4706,1.7029,0.0,0.4303}, 59 | {0.7182,0.6355,-0.0,2.4765,1.6681,0.0,0.422}, 60 | {0.732,0.6586,-0.0,2.4823,1.6333,0.0,0.4135}, 61 | {0.7458,0.6811,-0.0,2.488,1.5986,0.0,0.405}, 62 | {0.7597,0.7032,-0.0,2.4936,1.5638,0.0,0.3963}, 63 | {0.7735,0.7248,-0.0,2.4991,1.5291,0.0,0.3877}, 64 | {0.7873,0.7459,-0.0,2.5045,1.4943,0.0,0.379}, 65 | {0.8011,0.7666,-0.0,2.5097,1.4596,0.0,0.3704}, 66 | {0.8149,0.7867,-0.0,2.5148,1.4248,0.0,0.3617}, 67 | {0.8287,0.8064,-0.0,2.5198,1.3901,0.0,0.353}, 68 | {0.8425,0.8256,-0.0,2.5247,1.3553,0.0,0.3442}, 69 | {0.8563,0.8443,-0.0,2.5294,1.3206,0.0,0.3355}, 70 | {0.8702,0.8626,-0.0,2.5341,1.2858,0.0,0.3268}, 71 | {0.884,0.8803,-0.0,2.5386,1.2511,0.0,0.318}, 72 | {0.8978,0.8976,-0.0,2.543,1.2163,0.0,0.3093}, 73 | {0.9116,0.9144,-0.0,2.5473,1.1816,0.0,0.3005}, 74 | {0.9254,0.9307,-0.0,2.5514,1.1468,0.0,0.2917}, 75 | {0.9392,0.9466,-0.0,2.5554,1.1121,0.0,0.2829}, 76 | {0.953,0.9619,-0.0,2.5593,1.0773,0.0,0.2742}, 77 | {0.9668,0.9768,-0.0,2.5631,1.0426,0.0,0.2654}, 78 | {0.9807,0.9912,-0.0,2.5668,1.0078,0.0,0.2566}, 79 | {0.9945,1.0051,-0.0,2.5703,0.973,0.0,0.2478}, 80 | {1.0083,1.0186,-0.0,2.5738,0.9383,0.0,0.2389}, 81 | {1.0221,1.0315,-0.0,2.5771,0.9035,0.0,0.2301}, 82 | {1.0359,1.044,-0.0,2.5802,0.8688,0.0,0.2213}, 83 | {1.0497,1.056,-0.0,2.5833,0.834,0.0,0.2125}, 84 | {1.0635,1.0675,-0.0,2.5862,0.7993,0.0,0.2037}, 85 | {1.0773,1.0786,-0.0,2.589,0.7645,0.0,0.1948}, 86 | {1.0911,1.0891,-0.0,2.5917,0.7298,0.0,0.186}, 87 | {1.105,1.0992,-0.0,2.5943,0.695,0.0,0.1772}, 88 | {1.1188,1.1088,-0.0,2.5967,0.6603,0.0,0.1683}, 89 | {1.1326,1.1179,-0.0,2.5991,0.6255,0.0,0.1595}, 90 | {1.1464,1.1266,-0.0,2.6013,0.5908,0.0,0.1506}, 91 | {1.1602,1.1347,-0.0,2.6034,0.556,0.0,0.1418}, 92 | {1.174,1.1424,-0.0,2.6053,0.5213,0.0,0.1329}, 93 | {1.1878,1.1496,-0.0,2.6071,0.4865,0.0,0.1241}, 94 | {1.2016,1.1563,-0.0,2.6089,0.4518,0.0,0.1152}, 95 | {1.2155,1.1626,-0.0,2.6105,0.417,0.0,0.1063}, 96 | {1.2293,1.1683,-0.0,2.6119,0.3823,0.0,0.0975}, 97 | {1.2431,1.1736,-0.0,2.6133,0.3475,0.0,0.0886}, 98 | {1.2569,1.1784,-0.0,2.6145,0.3128,0.0,0.0798}, 99 | {1.2707,1.1827,-0.0,2.6156,0.278,0.0,0.0709}, 100 | {1.2845,1.1866,-0.0,2.6166,0.2433,0.0,0.062}, 101 | {1.2983,1.1899,-0.0,2.6174,0.2085,0.0,0.0532}, 102 | {1.3121,1.1928,-0.0,2.6182,0.1738,0.0,0.0443}, 103 | {1.326,1.1952,-0.0,2.6188,0.139,0.0,0.0355}, 104 | {1.3398,1.1971,-0.0,2.6193,0.1043,0.0,0.0266}, 105 | {1.3536,1.1986,-0.0,2.6196,0.0695,0.0,0.0177}, 106 | {1.3674,1.1995,-0.0,2.6199,0.0348,0.0,0.0089}, 107 | {1.3812,1.2,0.0,2.62,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/frc/paths/gogogogadget.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class gogogogadget extends Path { 6 | private final static double[][] points = { 7 | {0,0.0,0.0,0.314,-0.0,0.0,0.0}, 8 | {0.0197,-0.0,0.0,0.314,-0.0365,-0.0,-0.0063}, 9 | {0.0395,-0.0007,-0.0,0.3139,-0.0729,-0.0,-0.0126}, 10 | {0.0592,-0.0022,-0.0,0.3136,-0.1094,-0.0,-0.019}, 11 | {0.079,-0.0043,-0.0,0.3133,-0.1458,-0.0,-0.0253}, 12 | {0.0987,-0.0072,-0.0,0.3128,-0.1823,-0.0,-0.0316}, 13 | {0.1185,-0.0108,-0.0,0.3121,-0.2188,-0.0,-0.0379}, 14 | {0.1382,-0.0151,-0.0,0.3114,-0.2552,-0.0,-0.0442}, 15 | {0.158,-0.0202,-0.0,0.3105,-0.2917,-0.0,-0.0505}, 16 | {0.1777,-0.0259,-0.0,0.3095,-0.3281,-0.0,-0.0569}, 17 | {0.1975,-0.0324,-0.0,0.3084,-0.3646,-0.0,-0.0632}, 18 | {0.2172,-0.0396,-0.0,0.3071,-0.4011,-0.0,-0.0695}, 19 | {0.237,-0.0475,-0.0,0.3058,-0.4375,-0.0,-0.0758}, 20 | {0.2567,-0.0562,-0.0,0.3043,-0.474,-0.0,-0.0821}, 21 | {0.2765,-0.0655,-0.0,0.3026,-0.5105,-0.0,-0.0885}, 22 | {0.2962,-0.0756,-0.0,0.3009,-0.5469,-0.0,-0.0948}, 23 | {0.316,-0.0864,-0.0,0.299,-0.5834,-0.0,-0.1011}, 24 | {0.3357,-0.0979,-0.0,0.297,-0.6198,-0.0,-0.1074}, 25 | {0.3555,-0.1102,-0.0,0.2949,-0.6563,-0.0,-0.1137}, 26 | {0.3752,-0.1231,-0.0,0.2927,-0.6928,-0.0,-0.1201}, 27 | {0.3949,-0.1368,-0.0,0.2903,-0.7292,-0.0,-0.1264}, 28 | {0.4147,-0.1512,-0.0,0.2878,-0.7657,-0.0,-0.1327}, 29 | {0.4344,-0.1663,-0.0,0.2852,-0.8021,-0.0,-0.139}, 30 | {0.4542,-0.1822,-0.0,0.2824,-0.8386,-0.0,-0.1454}, 31 | {0.4739,-0.1987,-0.0,0.2796,-0.8751,-0.0,-0.1517}, 32 | {0.4937,-0.216,-0.0,0.2766,-0.9115,-0.0,-0.158}, 33 | {0.5134,-0.234,-0.0,0.2734,-0.948,-0.0,-0.1644}, 34 | {0.5332,-0.2527,-0.0,0.2702,-0.9844,-0.0,-0.1707}, 35 | {0.5529,-0.2722,-0.0,0.2668,-1.0209,-0.0,-0.177}, 36 | {0.5727,-0.2923,-0.0,0.2633,-1.0574,-0.0,-0.1834}, 37 | {0.5924,-0.3132,-0.0,0.2597,-1.0938,-0.0,-0.1897}, 38 | {0.6122,-0.3348,-0.0,0.256,-1.1303,-0.0,-0.1961}, 39 | {0.6319,-0.3571,-0.0,0.2521,-1.1667,-0.0,-0.2024}, 40 | {0.6517,-0.3802,-0.0,0.2481,-1.2032,-0.0,-0.2088}, 41 | {0.6714,-0.4039,-0.0,0.244,-1.2397,-0.0,-0.2151}, 42 | {0.6912,-0.4284,-0.0,0.2397,-1.2761,-0.0,-0.2215}, 43 | {0.7109,-0.4536,-0.0,0.2353,-1.3126,-0.0,-0.2278}, 44 | {0.7306,-0.4795,-0.0,0.2308,-1.3491,-0.0,-0.2342}, 45 | {0.7504,-0.5062,-0.0,0.2262,-1.3855,-0.0,-0.2405}, 46 | {0.7701,-0.5335,-0.0,0.2215,-1.422,-0.0,-0.2469}, 47 | {0.7899,-0.5616,-0.0,0.2166,-1.4584,-0.0,-0.2533}, 48 | {0.8096,-0.5904,-0.0,0.2116,-1.4949,-0.0,-0.2596}, 49 | {0.8294,-0.6199,-0.0,0.2065,-1.5314,-0.0,-0.266}, 50 | {0.8491,-0.6502,-0.0,0.2012,-1.5678,-0.0,-0.2724}, 51 | {0.8689,-0.6811,-0.0,0.1958,-1.6043,-0.0,-0.2788}, 52 | {0.8886,-0.7128,-0.0,0.1903,-1.6407,-0.0,-0.2852}, 53 | {0.9084,-0.7452,-0.0,0.1847,-1.6772,-0.0,-0.2916}, 54 | {0.9281,-0.7783,-0.0,0.1789,-1.7137,-0.0,-0.298}, 55 | {0.9479,-0.8122,-0.0,0.1731,-1.7501,-0.0,-0.3044}, 56 | {0.9676,-0.8467,-0.0,0.167,-1.7866,-0.0,-0.3109}, 57 | {0.9874,-0.882,-0.0,0.1609,-1.823,-0.0,-0.3174}, 58 | {1.0071,-0.918,-0.0,0.1546,-1.7866,0.0,-0.3127}, 59 | {1.0269,-0.9533,-0.0,0.1485,-1.7501,0.0,-0.3065}, 60 | {1.0466,-0.9878,-0.0,0.1424,-1.7137,0.0,-0.3002}, 61 | {1.0664,-1.0217,-0.0,0.1365,-1.6772,0.0,-0.2938}, 62 | {1.0861,-1.0548,-0.0,0.1307,-1.6407,0.0,-0.2875}, 63 | {1.1058,-1.0872,-0.0,0.125,-1.6043,0.0,-0.2812}, 64 | {1.1256,-1.1189,-0.0,0.1195,-1.5678,0.0,-0.2748}, 65 | {1.1453,-1.1498,-0.0,0.114,-1.5313,0.0,-0.2684}, 66 | {1.1651,-1.1801,-0.0,0.1087,-1.4949,0.0,-0.2621}, 67 | {1.1848,-1.2096,-0.0,0.1036,-1.4584,0.0,-0.2557}, 68 | {1.2046,-1.2384,-0.0,0.0985,-1.422,0.0,-0.2493}, 69 | {1.2243,-1.2665,-0.0,0.0936,-1.3855,0.0,-0.2429}, 70 | {1.2441,-1.2938,-0.0,0.0888,-1.349,0.0,-0.2366}, 71 | {1.2638,-1.3205,-0.0,0.0841,-1.3126,0.0,-0.2302}, 72 | {1.2836,-1.3464,-0.0,0.0796,-1.2761,0.0,-0.2238}, 73 | {1.3033,-1.3716,-0.0,0.0751,-1.2397,0.0,-0.2174}, 74 | {1.3231,-1.3961,-0.0,0.0709,-1.2032,0.0,-0.211}, 75 | {1.3428,-1.4198,-0.0,0.0667,-1.1667,0.0,-0.2046}, 76 | {1.3626,-1.4429,-0.0,0.0626,-1.1303,0.0,-0.1982}, 77 | {1.3823,-1.4652,-0.0,0.0587,-1.0938,0.0,-0.1919}, 78 | {1.4021,-1.4868,-0.0,0.0549,-1.0574,0.0,-0.1855}, 79 | {1.4218,-1.5077,-0.0,0.0513,-1.0209,0.0,-0.1791}, 80 | {1.4416,-1.5278,-0.0,0.0477,-0.9844,0.0,-0.1727}, 81 | {1.4613,-1.5473,-0.0,0.0443,-0.948,0.0,-0.1663}, 82 | {1.481,-1.566,-0.0,0.0411,-0.9115,0.0,-0.1599}, 83 | {1.5008,-1.584,-0.0,0.0379,-0.8751,0.0,-0.1535}, 84 | {1.5205,-1.6013,-0.0,0.0349,-0.8386,0.0,-0.1471}, 85 | {1.5403,-1.6178,-0.0,0.032,-0.8021,0.0,-0.1407}, 86 | {1.56,-1.6337,-0.0,0.0292,-0.7657,0.0,-0.1343}, 87 | {1.5798,-1.6488,-0.0,0.0265,-0.7292,0.0,-0.1279}, 88 | {1.5995,-1.6632,-0.0,0.024,-0.6928,0.0,-0.1215}, 89 | {1.6193,-1.6769,-0.0,0.0216,-0.6563,0.0,-0.1151}, 90 | {1.639,-1.6898,-0.0,0.0193,-0.6198,0.0,-0.1087}, 91 | {1.6588,-1.7021,-0.0,0.0172,-0.5834,0.0,-0.1023}, 92 | {1.6785,-1.7136,-0.0,0.0152,-0.5469,0.0,-0.0959}, 93 | {1.6983,-1.7244,-0.0,0.0133,-0.5104,0.0,-0.0896}, 94 | {1.718,-1.7345,-0.0,0.0115,-0.474,0.0,-0.0832}, 95 | {1.7378,-1.7438,-0.0,0.0099,-0.4375,0.0,-0.0768}, 96 | {1.7575,-1.7525,-0.0,0.0083,-0.4011,0.0,-0.0704}, 97 | {1.7773,-1.7604,-0.0,0.0069,-0.3646,0.0,-0.064}, 98 | {1.797,-1.7676,-0.0,0.0057,-0.3281,0.0,-0.0576}, 99 | {1.8167,-1.7741,-0.0,0.0045,-0.2917,0.0,-0.0512}, 100 | {1.8365,-1.7798,-0.0,0.0035,-0.2552,0.0,-0.0448}, 101 | {1.8562,-1.7849,-0.0,0.0027,-0.2188,0.0,-0.0384}, 102 | {1.876,-1.7892,-0.0,0.0019,-0.1823,0.0,-0.032}, 103 | {1.8957,-1.7928,-0.0,0.0013,-0.1458,0.0,-0.0256}, 104 | {1.9155,-1.7957,-0.0,0.0008,-0.1094,0.0,-0.0192}, 105 | {1.9352,-1.7978,-0.0,0.0004,-0.0729,0.0,-0.0128}, 106 | {1.955,-1.7993,-0.0,0.0001,-0.0365,0.0,-0.0064}, 107 | {1.9747,-1.8,0.0,0.0,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/Curve.java: -------------------------------------------------------------------------------- 1 | package frc.lib; 2 | 3 | /** 4 | * This class and its subclasses remap controller stick inputs according to 5 | * specifics provided of multiple classes of curves. 6 | * 7 | * This is the superclass of several types of curves. It contains most of the 8 | * code that is used for all types of curves (Linear, Exponential, Spline, and 9 | * Step). 10 | * 11 | * To create a Curve, do not create a Curve object directly, use a 12 | * subclass of Curve such as ExpCurve instead. 13 | * 14 | * @author Justin Babilino 15 | * @version 0.0.3 16 | */ 17 | public abstract class Curve { 18 | /** 19 | * The value added to the curve. 20 | */ 21 | private double offset; 22 | /** 23 | * The value multiplied to the curve. 24 | */ 25 | private double scalar; 26 | 27 | /** 28 | * The width of the deadband on the curve. 29 | */ 30 | private double deadzone; 31 | 32 | /** 33 | * Calculates and returns a mapped value based on the curve. 34 | * 35 | * @param input value to be mapped 36 | * @return mapped value 37 | */ 38 | public abstract double calculateMappedVal(double input); 39 | 40 | /** 41 | * Returns the value of input mapped to create 42 | * a deadband of width deadzone in the center of the 43 | * curve and squishes the rest of the curve to the outside 44 | * proportionally. 45 | * 46 | * @param input the input value to be mapped 47 | * @return mapped value 48 | */ 49 | protected double calculateDeadzone(double input) { 50 | double deadRadius = deadzone / 2.0; 51 | double val = 0.0; 52 | if (input > deadRadius) { 53 | val = (1.0 / (1.0 - deadRadius)) * (input - deadRadius); 54 | } else if (input < -deadRadius) { 55 | val = (1.0 / (1.0 - deadRadius)) * (input + deadRadius); 56 | } 57 | return val; 58 | } 59 | 60 | /** 61 | * Returns the value of input multiplied by the 62 | * value of scalar. 63 | * 64 | * @param input the input value to be mapped 65 | * @return mapped value 66 | */ 67 | protected double calculateScalar(double input) { 68 | double val = input * scalar; 69 | return val; 70 | } 71 | 72 | /** 73 | * Returns the value of input summed with the 74 | * value of offset. 75 | * 76 | * @param input the input value to be mapped 77 | * @return mapped value 78 | */ 79 | protected double calculateOffset(double input) { 80 | double val = input + offset; 81 | return val; 82 | } 83 | 84 | /** 85 | * Returns a set of points of length pointCount on the curve. 86 | * 87 | * @param pointCount the amount of points on the curve 88 | * @return a 2D double array of points on the curve 89 | */ 90 | public double[][] getCurvePoints(int pointCount) { 91 | double[][] points = new double[pointCount][2]; 92 | double dx = 2.0 / (pointCount - 1); 93 | for (int i = 0; i < pointCount; i++) { 94 | double x = -1.0 + (i * dx); 95 | points[i][0] = x; 96 | points[i][1] = calculateMappedVal(x); 97 | } 98 | return points; 99 | } 100 | 101 | /** 102 | * Prints the values of a 2D double array of points. The output of 103 | * this method can be pasted into https://www.desmos.com/calculator 104 | * to see a visual representation of the Curve. 105 | * 106 | * @param points the set of points to be printed 107 | */ 108 | public void printPoints(double[][] points) { 109 | System.out.println(); 110 | for (double[] point : points) { 111 | System.out.print("(" + point[0] + "," + point[1] + ")" + ","); 112 | } 113 | } 114 | 115 | /** 116 | * Prints a set of points on the curve of length pointCount. 117 | * The output of this method can be pasted into https://www.desmos.com/calculator 118 | * to see a visual representation of the Curve. 119 | * 120 | * @param pointCount the set of points to be printed 121 | */ 122 | public void printPoints(int pointCount) { 123 | printPoints(getCurvePoints(pointCount)); 124 | } 125 | 126 | /** 127 | * Sets the value of offset, the 128 | * value added to the final curve. 129 | * 130 | * @param offset the new value of offset 131 | */ 132 | public void setOffset(double offset) { 133 | this.offset = offset; 134 | } 135 | 136 | /** 137 | * Sets the value of scalar, the 138 | * value multiplied to the curve before it is offset. 139 | * 140 | * @param scalar the new value of scalar 141 | */ 142 | public void setScalar(double scalar) { 143 | this.scalar = scalar; 144 | } 145 | 146 | /** 147 | * Sets the value of deadzone, the 148 | * value for the width in the center of the curve 149 | * where any input results in an output of 0.0. 150 | * 151 | * @param deadzone the new value of deadzone 152 | */ 153 | public void setDeadzone(double deadzone) { 154 | this.deadzone = Math.abs(deadzone); 155 | } 156 | 157 | /** 158 | * Returns the value of offset, the double 159 | * value added to the final curve. 160 | * 161 | * @return the current value of offset 162 | */ 163 | public double getOffset() { 164 | return offset; 165 | } 166 | 167 | /** 168 | * Returns the value of scalar, the double 169 | * value multiplied to the curve before it is offset. 170 | * 171 | * @return the current value of scalar 172 | */ 173 | public double getScalar() { 174 | return scalar; 175 | } 176 | 177 | /** 178 | * Returns the value of deadzone, the 179 | * value for the width in the center of the curve 180 | * where any input results in an output of 0.0 181 | * 182 | * @return the current value of deadzone 183 | */ 184 | public double getDeadzone() { 185 | return deadzone; 186 | } 187 | } -------------------------------------------------------------------------------- /src/main/java/frc/paths/OnePointEightMetersForward.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class OnePointEightMetersForward extends Path { 6 | private final static double[][] points = { 7 | {0,0.0,0.0,0.0,-0.0,-0.0,0.0}, 8 | {0.0154,-0.0,-0.0,0.0,-0.0259,-0.0,-0.0039}, 9 | {0.0309,-0.0004,-0.0,-0.0001,-0.0518,-0.0,-0.0078}, 10 | {0.0463,-0.0012,-0.0,-0.0002,-0.0777,-0.0,-0.0117}, 11 | {0.0617,-0.0024,-0.0,-0.0004,-0.1037,-0.0,-0.0156}, 12 | {0.0772,-0.004,-0.0,-0.0006,-0.1296,-0.0,-0.0195}, 13 | {0.0926,-0.006,-0.0,-0.0009,-0.1555,-0.0,-0.0233}, 14 | {0.1081,-0.0084,-0.0,-0.0013,-0.1814,-0.0,-0.0272}, 15 | {0.1235,-0.0112,-0.0,-0.0017,-0.2073,-0.0,-0.0311}, 16 | {0.1389,-0.0144,-0.0,-0.0022,-0.2332,-0.0,-0.035}, 17 | {0.1544,-0.018,-0.0,-0.0027,-0.2591,-0.0,-0.0389}, 18 | {0.1698,-0.022,-0.0,-0.0033,-0.285,-0.0,-0.0428}, 19 | {0.1852,-0.0264,-0.0,-0.004,-0.311,-0.0,-0.0467}, 20 | {0.2007,-0.0312,-0.0,-0.0047,-0.3369,-0.0,-0.0506}, 21 | {0.2161,-0.0364,-0.0,-0.0055,-0.3628,-0.0,-0.0545}, 22 | {0.2315,-0.042,-0.0,-0.0063,-0.3887,-0.0,-0.0584}, 23 | {0.247,-0.048,-0.0,-0.0072,-0.4146,-0.0,-0.0623}, 24 | {0.2624,-0.0544,-0.0,-0.0082,-0.4405,-0.0,-0.0662}, 25 | {0.2778,-0.0612,-0.0,-0.0092,-0.4664,-0.0,-0.07}, 26 | {0.2933,-0.0684,-0.0,-0.0103,-0.4924,-0.0,-0.0739}, 27 | {0.3087,-0.076,-0.0,-0.0114,-0.5183,-0.0,-0.0778}, 28 | {0.3242,-0.084,-0.0,-0.0126,-0.5442,-0.0,-0.0817}, 29 | {0.3396,-0.0924,-0.0,-0.0139,-0.5701,-0.0,-0.0856}, 30 | {0.355,-0.1012,-0.0,-0.0152,-0.596,-0.0,-0.0895}, 31 | {0.3705,-0.1104,-0.0,-0.0166,-0.6219,-0.0,-0.0934}, 32 | {0.3859,-0.12,-0.0,-0.018,-0.6478,-0.0,-0.0973}, 33 | {0.4013,-0.13,-0.0,-0.0195,-0.6737,-0.0,-0.1012}, 34 | {0.4168,-0.1404,-0.0,-0.0211,-0.6997,-0.0,-0.1051}, 35 | {0.4322,-0.1512,-0.0,-0.0227,-0.7256,-0.0,-0.109}, 36 | {0.4476,-0.1624,-0.0,-0.0244,-0.7515,-0.0,-0.1128}, 37 | {0.4631,-0.174,-0.0,-0.0261,-0.7774,-0.0,-0.1167}, 38 | {0.4785,-0.186,-0.0,-0.0279,-0.8033,-0.0,-0.1206}, 39 | {0.494,-0.1984,-0.0,-0.0298,-0.8292,-0.0,-0.1245}, 40 | {0.5094,-0.2112,-0.0,-0.0317,-0.8551,-0.0,-0.1284}, 41 | {0.5248,-0.2244,-0.0,-0.0337,-0.8811,-0.0,-0.1323}, 42 | {0.5403,-0.238,-0.0,-0.0357,-0.907,-0.0,-0.1362}, 43 | {0.5557,-0.252,-0.0,-0.0378,-0.9329,-0.0,-0.1401}, 44 | {0.5711,-0.2664,-0.0,-0.04,-0.9588,-0.0,-0.144}, 45 | {0.5866,-0.2812,-0.0,-0.0422,-0.9847,-0.0,-0.1479}, 46 | {0.602,-0.2964,-0.0,-0.0445,-1.0106,-0.0,-0.1517}, 47 | {0.6174,-0.312,-0.0,-0.0469,-1.0365,-0.0,-0.1556}, 48 | {0.6329,-0.328,-0.0,-0.0493,-1.0625,-0.0,-0.1595}, 49 | {0.6483,-0.3444,-0.0,-0.0517,-1.0884,-0.0,-0.1634}, 50 | {0.6637,-0.3612,-0.0,-0.0542,-1.1143,-0.0,-0.1673}, 51 | {0.6792,-0.3784,-0.0,-0.0568,-1.1402,-0.0,-0.1712}, 52 | {0.6946,-0.396,-0.0,-0.0595,-1.1661,-0.0,-0.1751}, 53 | {0.7101,-0.414,-0.0,-0.0622,-1.192,-0.0,-0.1789}, 54 | {0.7255,-0.4324,-0.0,-0.0649,-1.2179,-0.0,-0.1828}, 55 | {0.7409,-0.4512,-0.0,-0.0677,-1.2438,-0.0,-0.1867}, 56 | {0.7564,-0.4704,-0.0,-0.0706,-1.2698,-0.0,-0.1906}, 57 | {0.7718,-0.49,-0.0,-0.0736,-1.2957,-0.0,-0.1944}, 58 | {0.7872,-0.51,-0.0,-0.0766,-1.2698,0.0,-0.1904}, 59 | {0.8027,-0.5296,-0.0,-0.0795,-1.2438,0.0,-0.1865}, 60 | {0.8181,-0.5488,-0.0,-0.0824,-1.2179,0.0,-0.1826}, 61 | {0.8335,-0.5676,-0.0,-0.0852,-1.192,0.0,-0.1787}, 62 | {0.849,-0.586,-0.0,-0.088,-1.1661,0.0,-0.1748}, 63 | {0.8644,-0.604,-0.0,-0.0907,-1.1402,0.0,-0.1709}, 64 | {0.8799,-0.6216,-0.0,-0.0933,-1.1143,0.0,-0.167}, 65 | {0.8953,-0.6388,-0.0,-0.0959,-1.0884,0.0,-0.1631}, 66 | {0.9107,-0.6556,-0.0,-0.0984,-1.0625,0.0,-0.1592}, 67 | {0.9262,-0.672,-0.0,-0.1009,-1.0365,0.0,-0.1553}, 68 | {0.9416,-0.688,-0.0,-0.1033,-1.0106,0.0,-0.1515}, 69 | {0.957,-0.7036,-0.0,-0.1056,-0.9847,0.0,-0.1476}, 70 | {0.9725,-0.7188,-0.0,-0.1079,-0.9588,0.0,-0.1437}, 71 | {0.9879,-0.7336,-0.0,-0.1101,-0.9329,0.0,-0.1398}, 72 | {1.0033,-0.748,-0.0,-0.1122,-0.907,0.0,-0.1359}, 73 | {1.0188,-0.762,-0.0,-0.1143,-0.8811,0.0,-0.132}, 74 | {1.0342,-0.7756,-0.0,-0.1164,-0.8551,0.0,-0.1281}, 75 | {1.0496,-0.7888,-0.0,-0.1184,-0.8292,0.0,-0.1242}, 76 | {1.0651,-0.8016,-0.0,-0.1203,-0.8033,0.0,-0.1204}, 77 | {1.0805,-0.814,-0.0,-0.1221,-0.7774,0.0,-0.1165}, 78 | {1.096,-0.826,-0.0,-0.1239,-0.7515,0.0,-0.1126}, 79 | {1.1114,-0.8376,-0.0,-0.1257,-0.7256,0.0,-0.1087}, 80 | {1.1268,-0.8488,-0.0,-0.1274,-0.6997,0.0,-0.1048}, 81 | {1.1423,-0.8596,-0.0,-0.129,-0.6738,0.0,-0.1009}, 82 | {1.1577,-0.87,-0.0,-0.1305,-0.6478,0.0,-0.097}, 83 | {1.1731,-0.88,-0.0,-0.132,-0.6219,0.0,-0.0932}, 84 | {1.1886,-0.8896,-0.0,-0.1335,-0.596,0.0,-0.0893}, 85 | {1.204,-0.8988,-0.0,-0.1348,-0.5701,0.0,-0.0854}, 86 | {1.2194,-0.9076,-0.0,-0.1362,-0.5442,0.0,-0.0815}, 87 | {1.2349,-0.916,-0.0,-0.1374,-0.5183,0.0,-0.0776}, 88 | {1.2503,-0.924,-0.0,-0.1386,-0.4924,0.0,-0.0738}, 89 | {1.2658,-0.9316,-0.0,-0.1398,-0.4664,0.0,-0.0699}, 90 | {1.2812,-0.9388,-0.0,-0.1408,-0.4405,0.0,-0.066}, 91 | {1.2966,-0.9456,-0.0,-0.1419,-0.4146,0.0,-0.0621}, 92 | {1.3121,-0.952,-0.0,-0.1428,-0.3887,0.0,-0.0582}, 93 | {1.3275,-0.958,-0.0,-0.1437,-0.3628,0.0,-0.0543}, 94 | {1.3429,-0.9636,-0.0,-0.1445,-0.3369,0.0,-0.0505}, 95 | {1.3584,-0.9688,-0.0,-0.1453,-0.311,0.0,-0.0466}, 96 | {1.3738,-0.9736,-0.0,-0.146,-0.285,0.0,-0.0427}, 97 | {1.3892,-0.978,-0.0,-0.1467,-0.2591,0.0,-0.0388}, 98 | {1.4047,-0.982,-0.0,-0.1473,-0.2332,0.0,-0.0349}, 99 | {1.4201,-0.9856,-0.0,-0.1478,-0.2073,0.0,-0.0311}, 100 | {1.4355,-0.9888,-0.0,-0.1483,-0.1814,0.0,-0.0272}, 101 | {1.451,-0.9916,-0.0,-0.1487,-0.1555,0.0,-0.0233}, 102 | {1.4664,-0.994,-0.0,-0.1491,-0.1296,0.0,-0.0194}, 103 | {1.4819,-0.996,-0.0,-0.1494,-0.1037,0.0,-0.0155}, 104 | {1.4973,-0.9976,-0.0,-0.1496,-0.0777,0.0,-0.0116}, 105 | {1.5127,-0.9988,-0.0,-0.1498,-0.0518,0.0,-0.0078}, 106 | {1.5282,-0.9996,-0.0,-0.1499,-0.0259,0.0,-0.0039}, 107 | {1.5436,-1.0,0.0,-0.15,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/frc/paths/FiveBallPartThree.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class FiveBallPartThree extends Path { 6 | private final static double[][] points = { 7 | {0,1.85,-0.5,-2.5416,-0.0,0.0,0.0}, 8 | {0.0264,1.85,-0.5,-2.5416,0.0653,0.0121,0.0015}, 9 | {0.0527,1.8517,-0.4997,-2.5416,0.1305,0.0243,0.003}, 10 | {0.0791,1.8552,-0.499,-2.5415,0.1958,0.0364,0.0046}, 11 | {0.1054,1.8603,-0.4981,-2.5414,0.2611,0.0486,0.0061}, 12 | {0.1318,1.8672,-0.4968,-2.5412,0.3263,0.0607,0.0076}, 13 | {0.1581,1.8758,-0.4952,-2.541,0.3916,0.0729,0.0091}, 14 | {0.1845,1.8861,-0.4933,-2.5407,0.4569,0.085,0.0107}, 15 | {0.2108,1.8982,-0.491,-2.5405,0.5221,0.0971,0.0122}, 16 | {0.2372,1.9119,-0.4885,-2.5401,0.5874,0.1093,0.0137}, 17 | {0.2635,1.9274,-0.4856,-2.5398,0.6527,0.1214,0.0152}, 18 | {0.2899,1.9446,-0.4824,-2.5394,0.7179,0.1336,0.0168}, 19 | {0.3162,1.9635,-0.4789,-2.5389,0.7832,0.1457,0.0183}, 20 | {0.3426,1.9842,-0.475,-2.5385,0.8485,0.1579,0.0198}, 21 | {0.3689,2.0065,-0.4709,-2.5379,0.9138,0.17,0.0213}, 22 | {0.3953,2.0306,-0.4664,-2.5374,0.979,0.1821,0.0229}, 23 | {0.4216,2.0564,-0.4616,-2.5368,1.0443,0.1943,0.0244}, 24 | {0.448,2.0839,-0.4565,-2.5361,1.1096,0.2064,0.0259}, 25 | {0.4744,2.1132,-0.451,-2.5354,1.1748,0.2186,0.0274}, 26 | {0.5007,2.1441,-0.4453,-2.5347,1.2401,0.2307,0.029}, 27 | {0.5271,2.1768,-0.4392,-2.534,1.3054,0.2429,0.0305}, 28 | {0.5534,2.2112,-0.4328,-2.5332,1.3706,0.255,0.032}, 29 | {0.5798,2.2473,-0.4261,-2.5323,1.4359,0.2671,0.0335}, 30 | {0.6061,2.2852,-0.419,-2.5314,1.5012,0.2793,0.0351}, 31 | {0.6325,2.3247,-0.4117,-2.5305,1.5664,0.2914,0.0366}, 32 | {0.6588,2.366,-0.404,-2.5295,1.6317,0.3036,0.0381}, 33 | {0.6852,2.409,-0.396,-2.5285,1.697,0.3157,0.0396}, 34 | {0.7115,2.4537,-0.3877,-2.5275,1.7622,0.3279,0.0412}, 35 | {0.7379,2.5002,-0.379,-2.5264,1.8275,0.34,0.0427}, 36 | {0.7642,2.5483,-0.3701,-2.5253,1.8928,0.3521,0.0442}, 37 | {0.7906,2.5982,-0.3608,-2.5241,1.958,0.3643,0.0457}, 38 | {0.8169,2.6498,-0.3512,-2.5229,2.0233,0.3764,0.0472}, 39 | {0.8433,2.7031,-0.3413,-2.5217,2.0886,0.3886,0.0488}, 40 | {0.8696,2.7582,-0.331,-2.5204,2.1538,0.4007,0.0503}, 41 | {0.896,2.8149,-0.3205,-2.5191,2.2191,0.4129,0.0518}, 42 | {0.9224,2.8734,-0.3096,-2.5177,2.2844,0.425,0.0533}, 43 | {0.9487,2.9336,-0.2984,-2.5163,2.3496,0.4371,0.0548}, 44 | {0.9751,2.9955,-0.2869,-2.5148,2.4149,0.4493,0.0564}, 45 | {1.0014,3.0592,-0.275,-2.5134,2.4802,0.4614,0.0579}, 46 | {1.0278,3.1245,-0.2629,-2.5118,2.5454,0.4736,0.0594}, 47 | {1.0541,3.1916,-0.2504,-2.5103,2.6107,0.4857,0.0609}, 48 | {1.0805,3.2604,-0.2376,-2.5087,2.676,0.4979,0.0624}, 49 | {1.1068,3.3309,-0.2245,-2.507,2.7413,0.51,0.0639}, 50 | {1.1332,3.4032,-0.211,-2.5053,2.8065,0.5221,0.0655}, 51 | {1.1595,3.4771,-0.1973,-2.5036,2.8718,0.5343,0.067}, 52 | {1.1859,3.5528,-0.1832,-2.5018,2.9371,0.5464,0.0685}, 53 | {1.2122,3.6302,-0.1688,-2.5,3.0023,0.5586,0.07}, 54 | {1.2386,3.7093,-0.1541,-2.4982,3.0676,0.5707,0.0715}, 55 | {1.2649,3.7902,-0.139,-2.4963,3.1329,0.5829,0.073}, 56 | {1.2913,3.8727,-0.1237,-2.4944,3.1981,0.595,0.0745}, 57 | {1.3176,3.957,-0.108,-2.4924,3.2634,0.6071,0.0759}, 58 | {1.344,4.043,-0.092,-2.4904,3.1981,0.595,0.0743}, 59 | {1.3704,4.1273,-0.0763,-2.4885,3.1329,0.5829,0.0727}, 60 | {1.3967,4.2098,-0.061,-2.4865,3.0676,0.5707,0.0712}, 61 | {1.4231,4.2907,-0.0459,-2.4847,3.0023,0.5586,0.0697}, 62 | {1.4494,4.3698,-0.0312,-2.4828,2.9371,0.5464,0.0681}, 63 | {1.4758,4.4472,-0.0168,-2.481,2.8718,0.5343,0.0666}, 64 | {1.5021,4.5229,-0.0027,-2.4793,2.8065,0.5221,0.0651}, 65 | {1.5285,4.5968,0.011,-2.4776,2.7413,0.51,0.0636}, 66 | {1.5548,4.6691,0.0245,-2.4759,2.676,0.4979,0.062}, 67 | {1.5812,4.7396,0.0376,-2.4743,2.6107,0.4857,0.0605}, 68 | {1.6075,4.8084,0.0504,-2.4727,2.5454,0.4736,0.059}, 69 | {1.6339,4.8755,0.0629,-2.4711,2.4802,0.4614,0.0575}, 70 | {1.6602,4.9408,0.075,-2.4696,2.4149,0.4493,0.056}, 71 | {1.6866,5.0045,0.0869,-2.4681,2.3496,0.4371,0.0544}, 72 | {1.7129,5.0664,0.0984,-2.4667,2.2844,0.425,0.0529}, 73 | {1.7393,5.1266,0.1096,-2.4653,2.2191,0.4129,0.0514}, 74 | {1.7656,5.1851,0.1205,-2.4639,2.1538,0.4007,0.0499}, 75 | {1.792,5.2418,0.131,-2.4626,2.0886,0.3886,0.0484}, 76 | {1.8184,5.2969,0.1413,-2.4613,2.0233,0.3764,0.0469}, 77 | {1.8447,5.3502,0.1512,-2.4601,1.958,0.3643,0.0454}, 78 | {1.8711,5.4018,0.1608,-2.4589,1.8928,0.3521,0.0438}, 79 | {1.8974,5.4517,0.1701,-2.4578,1.8275,0.34,0.0423}, 80 | {1.9238,5.4998,0.179,-2.4566,1.7622,0.3279,0.0408}, 81 | {1.9501,5.5463,0.1877,-2.4556,1.697,0.3157,0.0393}, 82 | {1.9765,5.591,0.196,-2.4545,1.6317,0.3036,0.0378}, 83 | {2.0028,5.634,0.204,-2.4535,1.5664,0.2914,0.0363}, 84 | {2.0292,5.6753,0.2117,-2.4526,1.5012,0.2793,0.0348}, 85 | {2.0555,5.7148,0.219,-2.4517,1.4359,0.2671,0.0332}, 86 | {2.0819,5.7527,0.2261,-2.4508,1.3706,0.255,0.0317}, 87 | {2.1082,5.7888,0.2328,-2.45,1.3054,0.2429,0.0302}, 88 | {2.1346,5.8232,0.2392,-2.4492,1.2401,0.2307,0.0287}, 89 | {2.1609,5.8559,0.2453,-2.4484,1.1748,0.2186,0.0272}, 90 | {2.1873,5.8868,0.251,-2.4477,1.1096,0.2064,0.0257}, 91 | {2.2136,5.9161,0.2565,-2.447,1.0443,0.1943,0.0242}, 92 | {2.24,5.9436,0.2616,-2.4464,0.979,0.1821,0.0227}, 93 | {2.2664,5.9694,0.2664,-2.4458,0.9138,0.17,0.0211}, 94 | {2.2927,5.9935,0.2709,-2.4452,0.8485,0.1579,0.0196}, 95 | {2.3191,6.0158,0.275,-2.4447,0.7832,0.1457,0.0181}, 96 | {2.3454,6.0365,0.2789,-2.4442,0.7179,0.1336,0.0166}, 97 | {2.3718,6.0554,0.2824,-2.4438,0.6527,0.1214,0.0151}, 98 | {2.3981,6.0726,0.2856,-2.4434,0.5874,0.1093,0.0136}, 99 | {2.4245,6.0881,0.2885,-2.443,0.5221,0.0971,0.0121}, 100 | {2.4508,6.1018,0.291,-2.4427,0.4569,0.085,0.0106}, 101 | {2.4772,6.1139,0.2933,-2.4424,0.3916,0.0729,0.0091}, 102 | {2.5035,6.1242,0.2952,-2.4422,0.3263,0.0607,0.0076}, 103 | {2.5299,6.1328,0.2968,-2.442,0.2611,0.0486,0.006}, 104 | {2.5562,6.1397,0.2981,-2.4418,0.1958,0.0364,0.0045}, 105 | {2.5826,6.1448,0.299,-2.4417,0.1305,0.0243,0.003}, 106 | {2.6089,6.1483,0.2997,-2.4416,0.0653,0.0121,0.0015}, 107 | {2.6353,6.15,0.3,-2.4416,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/frc/paths/NewAutoPartThree.java: -------------------------------------------------------------------------------- 1 | package frc.paths; 2 | 3 | import frc.lib.control.SwerveTrajectory; 4 | 5 | public class NewAutoPartThree extends Path { 6 | private final static double[][] points = { 7 | {0,4.85,0.4,-2.275,-0.0,0.0,-0.0}, 8 | {0.0273,4.85,0.4,-2.275,-0.0913,0.0056,0.0128}, 9 | {0.0545,4.8475,0.4002,-2.2747,-0.1827,0.0111,0.0256}, 10 | {0.0818,4.8425,0.4005,-2.274,-0.274,0.0167,0.0382}, 11 | {0.109,4.8351,0.4009,-2.2729,-0.3654,0.0223,0.0508}, 12 | {0.1363,4.8251,0.4015,-2.2715,-0.4567,0.0278,0.0633}, 13 | {0.1635,4.8127,0.4023,-2.2698,-0.548,0.0334,0.0757}, 14 | {0.1908,4.7977,0.4032,-2.2677,-0.6394,0.039,0.088}, 15 | {0.218,4.7803,0.4042,-2.2653,-0.7307,0.0445,0.1002}, 16 | {0.2453,4.7604,0.4055,-2.2626,-0.8221,0.0501,0.1123}, 17 | {0.2725,4.738,0.4068,-2.2595,-0.9134,0.0557,0.1242}, 18 | {0.2998,4.7131,0.4083,-2.2562,-1.0047,0.0612,0.1361}, 19 | {0.327,4.6857,0.41,-2.2524,-1.0961,0.0668,0.1478}, 20 | {0.3543,4.6558,0.4118,-2.2484,-1.1874,0.0724,0.1593}, 21 | {0.3816,4.6235,0.4138,-2.2441,-1.2788,0.0779,0.1707}, 22 | {0.4088,4.5886,0.4159,-2.2394,-1.3701,0.0835,0.1819}, 23 | {0.4361,4.5513,0.4182,-2.2345,-1.4615,0.0891,0.1929}, 24 | {0.4633,4.5114,0.4206,-2.2292,-1.5528,0.0946,0.2038}, 25 | {0.4906,4.4691,0.4232,-2.2237,-1.6442,0.1002,0.2144}, 26 | {0.5178,4.4243,0.4259,-2.2178,-1.7355,0.1058,0.2249}, 27 | {0.5451,4.377,0.4288,-2.2117,-1.8269,0.1113,0.235}, 28 | {0.5723,4.3272,0.4319,-2.2053,-1.9182,0.1169,0.245}, 29 | {0.5996,4.2749,0.435,-2.1986,-2.0096,0.1225,0.2546}, 30 | {0.6268,4.2202,0.4384,-2.1917,-2.1009,0.128,0.264}, 31 | {0.6541,4.1629,0.4419,-2.1845,-2.1923,0.1336,0.273}, 32 | {0.6813,4.1032,0.4455,-2.177,-2.2836,0.1392,0.2816}, 33 | {0.7086,4.0409,0.4493,-2.1694,-2.375,0.1448,0.2899}, 34 | {0.7359,3.9762,0.4532,-2.1615,-2.4664,0.1503,0.2977}, 35 | {0.7631,3.909,0.4573,-2.1533,-2.5577,0.1559,0.305}, 36 | {0.7904,3.8393,0.4616,-2.145,-2.6491,0.1615,0.3117}, 37 | {0.8176,3.7671,0.466,-2.1365,-2.7404,0.1671,0.3179}, 38 | {0.8449,3.6924,0.4705,-2.1279,-2.8318,0.1726,0.3233}, 39 | {0.8721,3.6152,0.4753,-2.1191,-2.9232,0.1782,0.3279}, 40 | {0.8994,3.5355,0.4801,-2.1101,-3.0146,0.1838,0.3315}, 41 | {0.9266,3.4534,0.4851,-2.1011,-3.1059,0.1894,0.3341}, 42 | {0.9539,3.3687,0.4903,-2.092,-3.1973,0.1949,0.3352}, 43 | {0.9811,3.2816,0.4956,-2.0828,-3.2887,0.2005,0.3347}, 44 | {1.0084,3.192,0.5011,-2.0737,-3.38,0.2061,0.3322}, 45 | {1.0356,3.0999,0.5067,-2.0647,-3.4714,0.2117,0.327}, 46 | {1.0629,3.0052,0.5124,-2.0558,-3.5628,0.2173,0.3183}, 47 | {1.0902,2.9081,0.5184,-2.0471,-3.6541,0.2228,0.3046}, 48 | {1.1174,2.8086,0.5244,-2.0388,-3.7454,0.2284,0.2834}, 49 | {1.1447,2.7065,0.5307,-2.0311,-3.8365,0.2339,0.2494}, 50 | {1.1719,2.6019,0.537,-2.0243,-3.927,0.2394,0.1893}, 51 | {1.1992,2.4949,0.5436,-2.0191,-4.0153,0.245,0.0747}, 52 | {1.2264,2.3855,0.5502,-2.0171,-4.0724,0.2483,0.0}, 53 | {1.2537,2.2745,0.557,-2.0171,-4.0724,0.2483,0.0}, 54 | {1.2809,2.1635,0.5638,-2.0171,-4.0724,0.2483,0.0}, 55 | {1.3082,2.0525,0.5705,-2.0171,-4.0724,0.2483,0.0}, 56 | {1.3354,1.9415,0.5773,-2.0171,-4.0724,0.2483,0.0}, 57 | {1.3627,1.8305,0.5841,-2.0171,-4.0724,0.2483,0.0}, 58 | {1.3899,1.7195,0.5908,-2.0171,-4.0724,0.2483,0.0}, 59 | {1.4172,1.6085,0.5976,-2.0171,-4.0724,0.2483,0.0}, 60 | {1.4445,1.4975,0.6044,-2.0171,-4.0724,0.2483,0.0}, 61 | {1.4717,1.3866,0.6112,-2.0171,-4.0724,0.2483,0.0}, 62 | {1.499,1.2756,0.6179,-2.0171,-4.0724,0.2483,0.0}, 63 | {1.5262,1.1646,0.6247,-2.0171,-4.0155,0.2451,0.0745}, 64 | {1.5535,1.0551,0.6314,-2.015,-3.9272,0.2395,0.1898}, 65 | {1.5807,0.9481,0.6379,-2.0099,-3.8366,0.234,0.2457}, 66 | {1.608,0.8435,0.6443,-2.0032,-3.7454,0.2285,0.2769}, 67 | {1.6352,0.7415,0.6505,-1.9956,-3.6541,0.2229,0.2962}, 68 | {1.6625,0.6419,0.6566,-1.9876,-3.5628,0.2173,0.3084}, 69 | {1.6897,0.5448,0.6625,-1.9791,-3.4715,0.2117,0.316}, 70 | {1.717,0.4502,0.6683,-1.9705,-3.3801,0.2062,0.3202}, 71 | {1.7442,0.358,0.6739,-1.9618,-3.2887,0.2006,0.322}, 72 | {1.7715,0.2684,0.6794,-1.953,-3.1973,0.195,0.3219}, 73 | {1.7988,0.1813,0.6847,-1.9443,-3.106,0.1894,0.3202}, 74 | {1.826,0.0966,0.6898,-1.9355,-3.0146,0.1839,0.3173}, 75 | {1.8533,0.0145,0.6948,-1.9269,-2.9232,0.1783,0.3134}, 76 | {1.8805,-0.0652,0.6997,-1.9183,-2.8318,0.1727,0.3086}, 77 | {1.9078,-0.1424,0.7044,-1.9099,-2.7405,0.1671,0.303}, 78 | {1.935,-0.2171,0.709,-1.9017,-2.6491,0.1616,0.2968}, 79 | {1.9623,-0.2893,0.7134,-1.8936,-2.5578,0.156,0.2901}, 80 | {1.9895,-0.359,0.7176,-1.8857,-2.4664,0.1504,0.2828}, 81 | {2.0168,-0.4262,0.7217,-1.878,-2.375,0.1448,0.2751}, 82 | {2.044,-0.4909,0.7257,-1.8705,-2.2837,0.1393,0.2671}, 83 | {2.0713,-0.5532,0.7295,-1.8632,-2.1923,0.1337,0.2586}, 84 | {2.0985,-0.6129,0.7331,-1.8561,-2.101,0.1281,0.2499}, 85 | {2.1258,-0.6702,0.7366,-1.8493,-2.0096,0.1225,0.2409}, 86 | {2.1531,-0.7249,0.7399,-1.8428,-1.9183,0.117,0.2316}, 87 | {2.1803,-0.7772,0.7431,-1.8365,-1.8269,0.1114,0.222}, 88 | {2.2076,-0.827,0.7462,-1.8304,-1.7355,0.1058,0.2123}, 89 | {2.2348,-0.8743,0.749,-1.8246,-1.6442,0.1003,0.2023}, 90 | {2.2621,-0.9191,0.7518,-1.8191,-1.5528,0.0947,0.1922}, 91 | {2.2893,-0.9614,0.7544,-1.8139,-1.4615,0.0891,0.1818}, 92 | {2.3166,-1.0013,0.7568,-1.8089,-1.3701,0.0835,0.1714}, 93 | {2.3438,-1.0386,0.7591,-1.8042,-1.2788,0.078,0.1607}, 94 | {2.3711,-1.0735,0.7612,-1.7999,-1.1875,0.0724,0.1499}, 95 | {2.3983,-1.1058,0.7632,-1.7958,-1.0961,0.0668,0.139}, 96 | {2.4256,-1.1357,0.765,-1.792,-1.0048,0.0613,0.128}, 97 | {2.4528,-1.1631,0.7667,-1.7885,-0.9134,0.0557,0.1168}, 98 | {2.4801,-1.188,0.7682,-1.7853,-0.8221,0.0501,0.1056}, 99 | {2.5074,-1.2104,0.7695,-1.7824,-0.7307,0.0446,0.0942}, 100 | {2.5346,-1.2303,0.7707,-1.7799,-0.6394,0.039,0.0827}, 101 | {2.5619,-1.2477,0.7718,-1.7776,-0.548,0.0334,0.0712}, 102 | {2.5891,-1.2627,0.7727,-1.7757,-0.4567,0.0278,0.0595}, 103 | {2.6164,-1.2751,0.7735,-1.7741,-0.3654,0.0223,0.0478}, 104 | {2.6436,-1.2851,0.7741,-1.7728,-0.274,0.0167,0.0359}, 105 | {2.6709,-1.2925,0.7745,-1.7718,-0.1827,0.0111,0.024}, 106 | {2.6981,-1.2975,0.7748,-1.7711,-0.0913,0.0056,0.0121}, 107 | {2.7254,-1.3,0.775,-1.7708,0.0,0.0,0.0}, 108 | }; 109 | public SwerveTrajectory getPath() { 110 | return new SwerveTrajectory(points); 111 | } 112 | } 113 | --------------------------------------------------------------------------------