From bbfa034a0c90b1852a209686303236bc6c20a2dc Mon Sep 17 00:00:00 2001 From: Thomas Date: Sat, 12 Mar 2022 08:50:16 -0500 Subject: [PATCH] BBBBBBBBBBBBBBBBBBBBBBBBBBBB --- src/main/java/frc/robot/Constants.java | 75 ++++--- src/main/java/frc/robot/Robot.java | 25 ++- src/main/java/frc/robot/RobotContainer.java | 56 ++++- src/main/java/frc/robot/SwervePod.java | 51 +++++ src/main/java/frc/robot/Utils.java | 13 ++ src/main/java/frc/robot/commands/Auto.java | 65 ++++++ src/main/java/frc/robot/commands/Blow.java | 32 +++ .../robot/commands/DriveWithJoysticks.java | 51 +++++ .../frc/robot/commands/ExampleCommand.java | 117 ----------- .../java/frc/robot/commands/IdleCommand.java | 21 ++ .../java/frc/robot/commands/SetShear.java | 34 +++ .../frc/robot/commands/SetShooterSpeed.java | 36 ++++ .../java/frc/robot/commands/ShearGoBrr.java | 32 +++ src/main/java/frc/robot/commands/Shoot.java | 43 ++++ .../java/frc/robot/commands/SnapAngle.java | 31 +++ src/main/java/frc/robot/commands/Succ.java | 32 +++ src/main/java/frc/robot/commands/Wait.java | 35 ++++ .../java/frc/robot/commands/ZeroGyro.java | 31 +++ .../java/frc/robot/subsystems/Chassis.java | 118 +++++++++++ .../robot/subsystems/ExampleSubsystem.java | 59 ------ .../java/frc/robot/subsystems/Gatherer.java | 194 ++++++++++++++++++ .../frc/robot/subsystems/PotatoCannon.java | 42 ++++ src/main/java/frc/robot/subsystems/Shear.java | 60 ++++++ vendordeps/navx_frc.json | 35 ++++ 24 files changed, 1071 insertions(+), 217 deletions(-) create mode 100644 src/main/java/frc/robot/SwervePod.java create mode 100644 src/main/java/frc/robot/Utils.java create mode 100644 src/main/java/frc/robot/commands/Auto.java create mode 100644 src/main/java/frc/robot/commands/Blow.java create mode 100644 src/main/java/frc/robot/commands/DriveWithJoysticks.java delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java create mode 100644 src/main/java/frc/robot/commands/IdleCommand.java create mode 100644 src/main/java/frc/robot/commands/SetShear.java create mode 100644 src/main/java/frc/robot/commands/SetShooterSpeed.java create mode 100644 src/main/java/frc/robot/commands/ShearGoBrr.java create mode 100644 src/main/java/frc/robot/commands/Shoot.java create mode 100644 src/main/java/frc/robot/commands/SnapAngle.java create mode 100644 src/main/java/frc/robot/commands/Succ.java create mode 100644 src/main/java/frc/robot/commands/Wait.java create mode 100644 src/main/java/frc/robot/commands/ZeroGyro.java create mode 100644 src/main/java/frc/robot/subsystems/Chassis.java delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/Gatherer.java create mode 100644 src/main/java/frc/robot/subsystems/PotatoCannon.java create mode 100644 src/main/java/frc/robot/subsystems/Shear.java create mode 100644 vendordeps/navx_frc.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0876917..87a047f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,53 +1,64 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; import edu.wpi.first.math.geometry.Translation2d; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ public final class Constants { - // public static int LF_DRIVE = 0; - // public static int LF_TURN = 1; - // public static int RF_DRIVE = 2; - // public static int RF_TURN = 3; - // public static int LB_DRIVE = 4; - // public static int LB_TURN = 5; - // public static int RB_DRIVE = 6; - // public static int RB_TURN = 7; + public static int LF_DRIVE = 1; + public static int LF_TURN = 2; + public static int RF_DRIVE = 3; + public static int RF_TURN = 4; + public static int LB_DRIVE = 5; + public static int LB_TURN = 6; + public static int RB_DRIVE = 7; + public static int RB_TURN = 8; - public static int LF_DRIVE = 2; - public static int LF_TURN = 1; - public static int RF_DRIVE = 4; - public static int RF_TURN = 3; - public static int LB_DRIVE = 6; - public static int LB_TURN = 5; - public static int RB_DRIVE = 8; - public static int RB_TURN = 7; + public static int SHOOTER = 10; + + public static int GATHER_LOW = 20; + public static int GATHER_HIGH_BACK = 21; + public static int GATHER_HIGH_FRONT = 22; + + public static int SHEAR_LEFT = 30; + public static int SHEAR_RIGHT = 31; + + public static int LF_POD_ZERO_DIO = 0; + public static int RF_POD_ZERO_DIO = 1; + public static int LB_POD_ZERO_DIO = 2; + public static int RB_POD_ZERO_DIO = 3; + public static int GATHER_LOW_DIO = 4; + public static int GATHER_HIGH_DIO = 5; public static double TURN_P = 1.0; public static double TURN_I = 0.0; public static double TURN_D = 0.0; + public static double SHEAR_P = 1.0; + public static double SHEAR_I = 0.0; + public static double SHEAR_D = 0.0; + public static double JOY_DRIVE_EXPO = 5.0; public static double JOY_TURN_EXPO = 5.0; - public static double POD_COUNTS_PER_DEG = 6656.0; + public static double SHOOTER_RPM = 1.0; + public static double SHOOTER_TARGET_ERROR = 0.1; + + public static double SHEAR_TOLERANCE = 0.1; + + // 2048 CPS * 2 (quadrature) + public static double SHOOTER_COUNTS_PER_REV = 2048.0 * 2.0; + + // 2048 CPS * 2 (quadrature) * 72 / 44 ratio + public static double POD_COUNTS_PER_REV = 2048.0 * 2.0 * 72.0 / 44.0; + + public static double SHEAR_COUNTS_PER_REV = 2048.0 * 2.0 * 100.0; // drive full speed meters - public static double DRIVE_MAX_SPEED = 5.0; + public static double DRIVE_MAX_SPEED = 9.144; + // public static double DRIVE_MAX_SPEED = 1.0; // meters - public static double swerveXFromCenter = 1.0; - public static double swerveYFromCenter = 2.0; + public static double swerveXFromCenter = 0.2794; + public static double swerveYFromCenter = 0.3175; public static Translation2d LF_TRANSLATION = new Translation2d(swerveXFromCenter, swerveYFromCenter); public static Translation2d RF_TRANSLATION = new Translation2d(swerveXFromCenter, -swerveYFromCenter); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f193cf3..851fe9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,10 +15,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; * project. */ public class Robot extends TimedRobot { + // private enum StartAngle { + // LEFT("Left"), + // CENTER("Center"), + // RIGHT("Right"); + + // private final String name; + // private String getDashName() { return name; } + + // StartAngle(String name) { this.name = name; } + // } + private Command m_autonomousCommand; + // public SendableChooser m_startAngle; private RobotContainer m_robotContainer; + public Robot() { + // m_startAngle = new SendableChooser(); + // m_startAngle.setDefaultOption(StartAngle.CENTER.getDashName(), StartAngle.CENTER); + // for (StartAngle angle : StartAngle.values()) { + // m_startAngle.addOption(angle.toString(), angle); + // } + // SmartDashboard.putData("Start angle", m_startAngle); + } + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -53,7 +74,9 @@ public class Robot extends TimedRobot { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + // System.out.println(m_robotContainer.m_exampleSubsystem.gyro.getAngle()); + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc8b41b..dac65f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,10 +5,24 @@ package frc.robot; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.Auto; +import frc.robot.commands.Blow; +import frc.robot.commands.DriveWithJoysticks; +import frc.robot.commands.SetShear; +import frc.robot.commands.SetShooterSpeed; +import frc.robot.commands.ShearGoBrr; +import frc.robot.commands.Shoot; +import frc.robot.commands.Succ; +import frc.robot.commands.ZeroGyro; +import frc.robot.subsystems.Chassis; +import frc.robot.subsystems.Gatherer; +import frc.robot.subsystems.PotatoCannon; +import frc.robot.subsystems.Shear; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -18,19 +32,24 @@ import edu.wpi.first.wpilibj2.command.Command; */ public class RobotContainer { // The robot's subsystems and commands are defined here... - public static XboxController s_driver = new XboxController(0); - public static XboxController s_manip = new XboxController(1); + public static PowerDistribution s_pdp = new PowerDistribution(); - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + public static Joystick s_driver = new Joystick(0); + public static Joystick s_manip = new Joystick(1); - private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + public final Chassis m_chassis = new Chassis(); + public final PotatoCannon m_potatoCannon = new PotatoCannon(); + public final Gatherer m_gatherer = new Gatherer(); + public final Shear m_shear = new Shear(); + + private final Auto m_autoCommand = new Auto(m_chassis, m_potatoCannon, m_gatherer); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - m_exampleSubsystem.setDefaultCommand(m_autoCommand); + m_chassis.setDefaultCommand(new DriveWithJoysticks(m_chassis)); } /** @@ -39,7 +58,28 @@ public class RobotContainer { * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() {} + private void configureButtonBindings() { + new JoystickButton(s_manip, 4).whenPressed(new SetShooterSpeed(m_potatoCannon, Constants.SHOOTER_RPM)); + new JoystickButton(s_manip, 3).whenPressed(new SetShooterSpeed(m_potatoCannon, Constants.SHOOTER_RPM/3.0*2.0)); + new JoystickButton(s_manip, 1).whenPressed(new SetShooterSpeed(m_potatoCannon, 0.4)); + new JoystickButton(s_manip, 2).whenPressed(new SetShooterSpeed(m_potatoCannon, 0.0)); + + new JoystickButton(s_driver, 6).whileHeld(new Succ(m_gatherer)); + new JoystickButton(s_driver, 8).whileHeld(new Blow(m_gatherer)); + + new JoystickButton(s_driver, 10).whenPressed(new ZeroGyro(m_chassis)); + + new JoystickButton(s_manip, 5).whileHeld(new Shoot(m_potatoCannon, m_gatherer)); + + new JoystickButton(s_manip, 9).whenPressed(new SetShear(m_shear, 90.0)); + + // new JoystickButton(s_manip, 10).whenPressed(new SequentialCommandGroup( + // new SetShear(m_shear, 90.0), + // new SetShear(m_shear, 45.0), + // new SetShear(m_shear, -45.0) + // )); + new JoystickButton(s_manip, 10).whileHeld(new ShearGoBrr(m_shear)); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/SwervePod.java b/src/main/java/frc/robot/SwervePod.java new file mode 100644 index 0000000..cc6f79e --- /dev/null +++ b/src/main/java/frc/robot/SwervePod.java @@ -0,0 +1,51 @@ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +public class SwervePod { + public TalonSRX turn; + public TalonFX drive; + + public SwervePod(int turn_id, int drive_id) { + turn = new TalonSRX(turn_id); + drive = new TalonFX(drive_id); + + turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + turn.setSensorPhase(true); + turn.config_kP(0, Constants.TURN_P); + turn.config_kI(0, Constants.TURN_I); + turn.config_kD(0, Constants.TURN_D); + + // drive.configPeakCurrentDuration(10); + drive.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 15, 0, 0.01)); + // drive.enableCurrentLimit(true);ddp + } + + /** + * Sets a pod to a specified angle and velocity + * + * @param pod: The pod to move + * @param angle: Pod angle in degrees + * @param velocity: Pod velocity in meters per second + */ + public void set(double angle, double velocity) { + // TODO: I can't remember if we waited for the angle target to be met before we + // start to apply the drive velocity, look into that + double drivePercent = velocity / Constants.DRIVE_MAX_SPEED; + if(Math.abs(drivePercent) > 0.05) + turn.set(ControlMode.Position, angle / 360.0 * Constants.POD_COUNTS_PER_REV); + drive.set(ControlMode.PercentOutput, drivePercent); + } + + public void zeroEncoder() { + turn.setSelectedSensorPosition(0); + } + + public double getAngle() { + return turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_REV * 360.0; + } +} diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java new file mode 100644 index 0000000..e9c32e2 --- /dev/null +++ b/src/main/java/frc/robot/Utils.java @@ -0,0 +1,13 @@ +package frc.robot; + +public class Utils { + public static double joyExpo(double in, double exp) { + double out; + + out = Math.exp(Math.abs(in) * exp) - 1; + out = out / (Math.exp(exp) - 1); + out = Math.signum(in) * out; + + return out; + } +} diff --git a/src/main/java/frc/robot/commands/Auto.java b/src/main/java/frc/robot/commands/Auto.java new file mode 100644 index 0000000..f0b6b6a --- /dev/null +++ b/src/main/java/frc/robot/commands/Auto.java @@ -0,0 +1,65 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Chassis; +import frc.robot.subsystems.Gatherer; +import frc.robot.subsystems.PotatoCannon; + +public class Auto extends CommandBase { + private Chassis m_chassis; + private PotatoCannon m_cannon; + private Gatherer m_gatherer; + + private double m_startTime; + + public Auto(Chassis chassis, PotatoCannon cannon, Gatherer gatherer) { + addRequirements(chassis); + addRequirements(cannon); + addRequirements(gatherer); + m_chassis = chassis; + m_cannon = cannon; + m_gatherer = gatherer; + } + + @Override + public void initialize() { + m_startTime = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + double curTime = Timer.getFPGATimestamp(); + double time = curTime - m_startTime; + m_cannon.set(0.95); + if(time > 7.0) { + // System.out.println("Stop"); + m_chassis.driveFR(0.0, 0.0, 0.0); + m_cannon.set(0); + m_gatherer.stop(); + } else if(time > 4.0) { + // System.out.println("Move"); + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + 2.0, + 0.0, + 0.0, + Rotation2d.fromDegrees(0.0)); + m_chassis.drive(speeds); + // m_chassis.driveFR(2.0, 0.0, 0.0); + } else if(time > 2.0) { + // System.out.println("Shoot"); + m_gatherer.shoot(false); + } + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Blow.java b/src/main/java/frc/robot/commands/Blow.java new file mode 100644 index 0000000..d64b160 --- /dev/null +++ b/src/main/java/frc/robot/commands/Blow.java @@ -0,0 +1,32 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Gatherer; + +public class Blow extends CommandBase { + private Gatherer m_gatherer; + + public Blow(Gatherer gatherer) { + addRequirements(gatherer); + m_gatherer = gatherer; + } + + @Override + public void initialize() { + m_gatherer.eject(); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + m_gatherer.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java new file mode 100644 index 0000000..20829e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -0,0 +1,51 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.Utils; +import frc.robot.subsystems.Chassis; + +public class DriveWithJoysticks extends CommandBase { + private final Chassis m_chassis; + + public DriveWithJoysticks(Chassis chassis) { + addRequirements(chassis); + m_chassis = chassis; + + } + + @Override + public void initialize() { + // m_chassis.zeroPodEncoders(); + // m_chassis.zeroGyro(); + } + + @Override + public void execute() { + // Get joystick values + double x = RobotContainer.s_driver.getY() * 2.0; + double y = RobotContainer.s_driver.getX() * 2.0; + double theta = RobotContainer.s_driver.getZ() * 1.5; + + // Apply exponential to allow finer driver control + x = Utils.joyExpo(x, Constants.JOY_DRIVE_EXPO); + y = Utils.joyExpo(y, Constants.JOY_DRIVE_EXPO); + theta = Utils.joyExpo(theta, Constants.JOY_DRIVE_EXPO); + + // TODO: Move this crap + // double total = 0.0; + // for(int i = 0; i < 16; i++) + // total += RobotContainer.s_pdp.getCurrent(i); + // System.out.println("A: " + total + ", W: " + RobotContainer.s_pdp.getTotalPower()); + m_chassis.driveFR(x, y, theta); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 16eaadf..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.ExampleSubsystem; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - private SwerveDriveKinematics m_kinematics; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - - m_kinematics = new SwerveDriveKinematics(Constants.LF_TRANSLATION, Constants.RF_TRANSLATION, Constants.LB_TRANSLATION, Constants.RB_TRANSLATION); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_subsystem.lf_turn.setSelectedSensorPosition(0); - System.out.println("Cmd Init"); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - - double x = RobotContainer.s_driver.getLeftY(); - double y = RobotContainer.s_driver.getLeftX(); - double theta = RobotContainer.s_driver.getRightX(); - - x = joyExpo(x, Constants.JOY_DRIVE_EXPO); - y = joyExpo(y, Constants.JOY_DRIVE_EXPO); - theta = joyExpo(theta, Constants.JOY_DRIVE_EXPO); - - System.out.print(x); - System.out.print(", "); - System.out.print(y); - System.out.print(", "); - System.out.println(theta); - - ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(x, y, theta, Rotation2d.fromDegrees(0)); - - SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); - - SwerveModuleState lf_optimized = SwerveModuleState.optimize( - moduleStates[0], - Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0) - ); - SwerveModuleState rf_optimized = SwerveModuleState.optimize( - moduleStates[1], - Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0) - ); - SwerveModuleState lb_optimized = SwerveModuleState.optimize( - moduleStates[2], - Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0) - ); - SwerveModuleState rb_optimized = SwerveModuleState.optimize( - moduleStates[3], - Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0) - ); - - m_subsystem.lf_turn.set(ControlMode.Position, lf_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG); - m_subsystem.rf_turn.set(ControlMode.Position, rf_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG); - m_subsystem.lb_turn.set(ControlMode.Position, lb_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG); - m_subsystem.rb_turn.set(ControlMode.Position, rb_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG); - - System.out.println(lf_optimized); - m_subsystem.lf_drive.set(ControlMode.PercentOutput, lf_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED); - m_subsystem.rf_drive.set(ControlMode.PercentOutput, rf_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED); - m_subsystem.lb_drive.set(ControlMode.PercentOutput, lb_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED); - m_subsystem.rb_drive.set(ControlMode.PercentOutput, rb_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED); - } - - private double joyExpo(double in, double exp) { - double out; - - out = Math.exp(Math.abs(in) * exp) - 1; - out = out / (Math.exp(exp) - 1); - out = Math.signum(in) * out; - - return out; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/IdleCommand.java b/src/main/java/frc/robot/commands/IdleCommand.java new file mode 100644 index 0000000..3be58b1 --- /dev/null +++ b/src/main/java/frc/robot/commands/IdleCommand.java @@ -0,0 +1,21 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IdleCommand extends CommandBase { + public IdleCommand() {} + + @Override + public void initialize() {} + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SetShear.java b/src/main/java/frc/robot/commands/SetShear.java new file mode 100644 index 0000000..3167215 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetShear.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Shear; + +public class SetShear extends CommandBase { + private Shear m_shear; + + private double m_angle; + + public SetShear(Shear shear, double angle) { + addRequirements(shear); + m_shear = shear; + m_angle = angle; + } + + @Override + public void initialize() { + m_shear.set(m_angle); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return m_shear.atTarget(); + } +} diff --git a/src/main/java/frc/robot/commands/SetShooterSpeed.java b/src/main/java/frc/robot/commands/SetShooterSpeed.java new file mode 100644 index 0000000..2620a4c --- /dev/null +++ b/src/main/java/frc/robot/commands/SetShooterSpeed.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.PotatoCannon; + +public class SetShooterSpeed extends CommandBase { + private PotatoCannon m_cannon; + private double m_speed; + + public SetShooterSpeed(PotatoCannon cannon, double speed) { + addRequirements(cannon); + m_cannon = cannon; + m_speed = speed; + // System.out.println("WOT " + speed); + } + + @Override + public void initialize() { + // if(m_speed > 0) + // m_cannon.a+=0.1; + // else + // m_cannon.a-=0.1; + m_cannon.set(m_speed); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/ShearGoBrr.java b/src/main/java/frc/robot/commands/ShearGoBrr.java new file mode 100644 index 0000000..ed62194 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShearGoBrr.java @@ -0,0 +1,32 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Shear; + +public class ShearGoBrr extends CommandBase { + private Shear m_shear; + + public ShearGoBrr(Shear shear) { + addRequirements(shear); + m_shear = shear; + } + + @Override + public void initialize() { + m_shear.brr(); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + m_shear.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Shoot.java b/src/main/java/frc/robot/commands/Shoot.java new file mode 100644 index 0000000..b555d92 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Gatherer; +import frc.robot.subsystems.PotatoCannon; + +public class Shoot extends CommandBase { + private PotatoCannon m_cannon; + private Gatherer m_gatherer; + + private double m_startTime; + + public Shoot(PotatoCannon cannon, Gatherer gatherer) { + addRequirements(cannon, gatherer); + m_cannon = cannon; + m_gatherer = gatherer; + } + + @Override + public void initialize() { + m_startTime = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + // if(m_cannon.isAtSpeed()) { + m_gatherer.shoot(false); + if(Timer.getFPGATimestamp() - m_startTime > 1.0) { + m_gatherer.shoot(true); + } + } + + @Override + public void end(boolean interrupted) { + m_gatherer.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SnapAngle.java b/src/main/java/frc/robot/commands/SnapAngle.java new file mode 100644 index 0000000..ab3319c --- /dev/null +++ b/src/main/java/frc/robot/commands/SnapAngle.java @@ -0,0 +1,31 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Chassis; + +public class SnapAngle extends CommandBase { + private Chassis m_chassis; + + public SnapAngle(Chassis chassis) { + addRequirements(chassis); + m_chassis = chassis; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + // if(m_cannon.isAtSpeed()) { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Succ.java b/src/main/java/frc/robot/commands/Succ.java new file mode 100644 index 0000000..6b23124 --- /dev/null +++ b/src/main/java/frc/robot/commands/Succ.java @@ -0,0 +1,32 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Gatherer; + +public class Succ extends CommandBase { + private Gatherer m_gatherer; + + public Succ(Gatherer gatherer) { + addRequirements(gatherer); + m_gatherer = gatherer; + } + + @Override + public void initialize() { + m_gatherer.gather(); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + m_gatherer.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Wait.java b/src/main/java/frc/robot/commands/Wait.java new file mode 100644 index 0000000..e81f4ce --- /dev/null +++ b/src/main/java/frc/robot/commands/Wait.java @@ -0,0 +1,35 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Wait extends CommandBase { + private double m_startTime; + private double m_time; + + public Wait(SubsystemBase subsystem, double time) { + addRequirements(subsystem); + + m_time = time; + } + + @Override + public void initialize() { + m_startTime = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + double curTime = Timer.getFPGATimestamp(); + return curTime - m_startTime > m_time; + } +} diff --git a/src/main/java/frc/robot/commands/ZeroGyro.java b/src/main/java/frc/robot/commands/ZeroGyro.java new file mode 100644 index 0000000..50993af --- /dev/null +++ b/src/main/java/frc/robot/commands/ZeroGyro.java @@ -0,0 +1,31 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Chassis; + +public class ZeroGyro extends CommandBase { + private Chassis m_chassis; + + public ZeroGyro(Chassis chassis) { + addRequirements(chassis); + m_chassis = chassis; + } + + @Override + public void initialize() { + m_chassis.zeroGyro(); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/Chassis.java b/src/main/java/frc/robot/subsystems/Chassis.java new file mode 100644 index 0000000..1d2f3fb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Chassis.java @@ -0,0 +1,118 @@ +package frc.robot.subsystems; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.SPI.Port; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.SwervePod; + +public class Chassis extends SubsystemBase { + public enum Pod { + LEFT_FRONT, + RIGHT_FRONT, + LEFT_BACK, + RIGHT_BACK + } + + private SwerveDriveKinematics m_kinematics; + + private SwervePod m_lf_pod = new SwervePod(Constants.LF_TURN, Constants.LF_DRIVE); + private SwervePod m_rf_pod = new SwervePod(Constants.RF_TURN, Constants.RF_DRIVE); + private SwervePod m_lb_pod = new SwervePod(Constants.LB_TURN, Constants.LB_DRIVE); + private SwervePod m_rb_pod = new SwervePod(Constants.RB_TURN, Constants.RB_DRIVE); + + public AHRS m_gyro = new AHRS(Port.kMXP); + + public Chassis() { + m_kinematics = new SwerveDriveKinematics( + Constants.LF_TRANSLATION, + Constants.RF_TRANSLATION, + Constants.LB_TRANSLATION, + Constants.RB_TRANSLATION); + + zeroPodEncoders(); + // zeroGyro(); + } + + public void drive(ChassisSpeeds speeds) { + SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); + + // Optimize pod angles so we arnt doing stupid wrapping + SwerveModuleState lf_optimized = SwerveModuleState.optimize( + moduleStates[0], + Rotation2d.fromDegrees(getPod(Pod.LEFT_FRONT).getAngle()) + ); + SwerveModuleState rf_optimized = SwerveModuleState.optimize( + moduleStates[1], + Rotation2d.fromDegrees(getPod(Pod.RIGHT_FRONT).getAngle()) + ); + SwerveModuleState lb_optimized = SwerveModuleState.optimize( + moduleStates[2], + Rotation2d.fromDegrees(getPod(Pod.LEFT_BACK).getAngle()) + ); + SwerveModuleState rb_optimized = SwerveModuleState.optimize( + moduleStates[3], + Rotation2d.fromDegrees(getPod(Pod.RIGHT_BACK).getAngle()) + ); + + // Set the pod's angle and velocity + getPod(Pod.LEFT_FRONT).set( + lf_optimized.angle.getDegrees(), + lf_optimized.speedMetersPerSecond); + getPod(Pod.RIGHT_FRONT).set( + rf_optimized.angle.getDegrees(), + rf_optimized.speedMetersPerSecond); + getPod(Pod.LEFT_BACK).set( + lb_optimized.angle.getDegrees(), + lb_optimized.speedMetersPerSecond); + getPod(Pod.RIGHT_BACK).set( + rb_optimized.angle.getDegrees(), + rb_optimized.speedMetersPerSecond); + } + + public void driveFR(double x, double y, double theta) { + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + x, + y, + theta, + Rotation2d.fromDegrees(-m_gyro.getAngle())); + drive(speeds); + } + + public void zeroPodEncoders() { + m_lf_pod.zeroEncoder(); + m_rf_pod.zeroEncoder(); + m_lb_pod.zeroEncoder(); + m_rb_pod.zeroEncoder(); + } + + public void zeroGyro() { + m_gyro.zeroYaw(); + } + + public SwervePod getPod(Pod pod) { + switch(pod) { + case LEFT_FRONT: + return m_lf_pod; + case RIGHT_FRONT: + return m_rf_pod; + case LEFT_BACK: + return m_lb_pod; + case RIGHT_BACK: + return m_rb_pod; + default: + throw new RuntimeException("Invalid pod?"); + } + } + + @Override + public void periodic() {} + + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index b6f8975..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class ExampleSubsystem extends SubsystemBase { - public TalonSRX lf_drive = new TalonSRX(Constants.LF_DRIVE); - public TalonSRX lf_turn = new TalonSRX(Constants.LF_TURN); - public TalonSRX rf_drive = new TalonSRX(Constants.RF_DRIVE); - public TalonSRX rf_turn = new TalonSRX(Constants.RF_TURN); - public TalonSRX lb_drive = new TalonSRX(Constants.LB_DRIVE); - public TalonSRX lb_turn = new TalonSRX(Constants.LB_TURN); - public TalonSRX rb_drive = new TalonSRX(Constants.RB_DRIVE); - public TalonSRX rb_turn = new TalonSRX(Constants.RB_TURN); - - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() { - lf_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - lf_turn.setSensorPhase(true); - lf_turn.config_kP(0, Constants.TURN_P); - lf_turn.config_kI(0, Constants.TURN_I); - lf_turn.config_kD(0, Constants.TURN_D); - - rf_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - rf_turn.setSensorPhase(true); - rf_turn.config_kP(0, Constants.TURN_P); - rf_turn.config_kI(0, Constants.TURN_I); - rf_turn.config_kD(0, Constants.TURN_D); - - lb_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - lb_turn.setSensorPhase(true); - lb_turn.config_kP(0, Constants.TURN_P); - lb_turn.config_kI(0, Constants.TURN_I); - lb_turn.config_kD(0, Constants.TURN_D); - - rb_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - rb_turn.setSensorPhase(true); - rb_turn.config_kP(0, Constants.TURN_P); - rb_turn.config_kI(0, Constants.TURN_I); - rb_turn.config_kD(0, Constants.TURN_D); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/Gatherer.java b/src/main/java/frc/robot/subsystems/Gatherer.java new file mode 100644 index 0000000..9b5beea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gatherer.java @@ -0,0 +1,194 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Gatherer extends SubsystemBase { + private enum GatherState { + IDLE, + WAITING + } + + private enum ManageState { + EMPTY, + ENTERING_LOW, + TRANSFERRING, + LEAVING_HIGH + } + + private enum BallState { + NO_BALLS, + ENTERING_LOW, + LOW, + TRANSFERRING, + HIGH, + TWO_BALLS, + SATURATED + } + + private TalonSRX m_gather_low = new TalonSRX(Constants.GATHER_LOW); + private TalonSRX m_gather_high_back = new TalonSRX(Constants.GATHER_HIGH_BACK); + private TalonSRX m_gather_high_front = new TalonSRX(Constants.GATHER_HIGH_FRONT); + + private DigitalInput m_lowSensor = new DigitalInput(Constants.GATHER_LOW_DIO); + private DigitalInput m_highSensor = new DigitalInput(Constants.GATHER_HIGH_DIO); + + private int m_ballsInTransport = 0; + + private boolean m_prevLowSensor = false; + private boolean m_prevHighSensor = false; + + private boolean m_haveLowBall = false; + private boolean m_haveHighBall = false; + + private BallState m_BallState = BallState.NO_BALLS; + private GatherState m_state = GatherState.IDLE; + private ManageState m_manageState = ManageState.EMPTY; + + public double shoot_speed_front = 1.0; + public double shoot_speed_back = 0.5; + + public Gatherer() { + m_gather_low.setInverted(false); + m_gather_high_back.setInverted(true); + m_gather_high_front.setInverted(false); + } + + public boolean haveOneBall() { + return !m_lowSensor.get(); + } + + public boolean haveTwoBall() { + return false; + } + + public void gather() { + // m_state = GatherState.GATHERING; + setGather(1); + // setTower(0.6); + m_gather_high_back.set(ControlMode.PercentOutput, -0.6); + m_gather_high_front.set(ControlMode.PercentOutput, 0.6); + } + + public void store() { + setGather(1); + m_gather_high_back.set(ControlMode.PercentOutput, shoot_speed_back); + m_gather_high_front.set(ControlMode.PercentOutput, shoot_speed_front); + } + + public void eject() { + // m_state = GatherState.EJECTING; + setGather(-1); + setTower(-0.6); + } + + public void shoot(boolean full) { + // m_state = GatherState.SHOOTING; + // setTower(1); + + if(full) { + setGather(1.0); + } + + m_gather_high_back.set(ControlMode.PercentOutput, shoot_speed_back); + m_gather_high_front.set(ControlMode.PercentOutput, shoot_speed_front); + } + + public void stop() { + m_state = GatherState.IDLE; + setGather(0); + setTower(0); + } + + public void setGather(double speed) { + m_gather_low.set(ControlMode.PercentOutput, speed); + } + + public void setTower(double speed) { + m_gather_high_back.set(ControlMode.PercentOutput, speed); + m_gather_high_front.set(ControlMode.PercentOutput, speed); + } + + @Override + public void periodic() { + boolean lowSensor = m_lowSensor.get(); + boolean highSensor = m_highSensor.get(); + + boolean lowStateChanged = false; + boolean highStateChanged = false; + + if(m_prevLowSensor != lowSensor) { + lowStateChanged = true; + m_prevLowSensor = lowSensor; + } + if(m_prevHighSensor != highSensor) { + highStateChanged = true; + m_prevHighSensor = highSensor; + } + + switch(m_BallState) { + case NO_BALLS: + break; + case ENTERING_LOW: + // setGather(1); + break; + case LOW: + break; + case TRANSFERRING: + break; + case HIGH: + break; + case TWO_BALLS: + break; + case SATURATED: + break; + } + + // switch(m_state) { + // case IDLE: + // setGather(0); + // setTower(0); + // break; + // case GATHERING: + // setGather(1); + // break; + // case EJECTING: + // setGather(-1); + // break; + // case SHOOTING: + // setTower(1); + // break; + // case MANAGING: + // manage(); + // break; + // } + } + + private void manage() { + + switch(m_manageState) { + case EMPTY: + break; + case ENTERING_LOW: + setGather(1); + // if(lowStateChanged && lowSensor) { + // setGather(0); + // m_haveLowBall = true; + // } + break; + case TRANSFERRING: + break; + case LEAVING_HIGH: + break; + } + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/PotatoCannon.java b/src/main/java/frc/robot/subsystems/PotatoCannon.java new file mode 100644 index 0000000..bc51f4f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PotatoCannon.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class PotatoCannon extends SubsystemBase { + public TalonSRX m_shooter = new TalonSRX(Constants.SHOOTER); + + public double a = 0.0; + + public PotatoCannon() { + m_shooter.setInverted(false); + // m_shooter.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + // m_shooter.setSensorPhase(true); + // m_shooter.config_kP(0, Constants.TURN_P); + // m_shooter.config_kI(0, Constants.TURN_I); + // m_shooter.config_kD(0, Constants.TURN_D); + } + + public void set(double rpm) { + // m_shooter.set(ControlMode.Velocity, rpm * Constants.SHOOTER_COUNTS_PER_REV); + m_shooter.set(ControlMode.PercentOutput, rpm); + } + + public boolean isAtSpeed() { + return Math.abs(m_shooter.getClosedLoopError()) < Constants.SHOOTER_TARGET_ERROR; + } + + @Override + public void periodic() { + // m_shooter.set(ControlMode.PercentOutput, 0.1); + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/Shear.java b/src/main/java/frc/robot/subsystems/Shear.java new file mode 100644 index 0000000..380cca4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shear.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Shear extends SubsystemBase { + public TalonSRX m_left = new TalonSRX(Constants.SHEAR_LEFT); + public TalonSRX m_right = new TalonSRX(Constants.SHEAR_RIGHT); + + public Shear() { + m_left.setInverted(true); + // m_right.setInverted(true); + + m_left.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + // m_left.setSensorPhase(true); + m_left.config_kP(0, Constants.SHEAR_P); + m_left.config_kI(0, Constants.SHEAR_I); + m_left.config_kD(0, Constants.SHEAR_D); + + m_right.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + // m_right.setSensorPhase(true); + m_right.config_kP(0, Constants.SHEAR_P); + m_right.config_kI(0, Constants.SHEAR_I); + m_right.config_kD(0, Constants.SHEAR_D); + } + + public void set(double angle) { + // m_shooter.set(ControlMode.Velocity, rpm * Constants.SHOOTER_COUNTS_PER_REV); + m_left.set(ControlMode.Position, angle); + m_right.set(ControlMode.Position, angle); + } + + public boolean atTarget() { + return (m_left.getClosedLoopError() < Constants.SHEAR_TOLERANCE) && (m_right.getClosedLoopError() < Constants.SHEAR_TOLERANCE); + } + + public void brr() { + m_left.set(ControlMode.PercentOutput, 1.0); + m_right.set(ControlMode.PercentOutput, 1.0); + } + + public void stop() { + m_left.set(ControlMode.PercentOutput, 0.0); + m_right.set(ControlMode.PercentOutput, 0.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..0fb49a7 --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,35 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "4.0.442", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "4.0.442" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "4.0.442", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file