diff --git a/.vscode/launch.json b/.vscode/launch.json index 893bd6c..8b86b6f 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,12 +4,18 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "RA22_RobotCode" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", diff --git a/build.gradle b/build.gradle index 104b058..973c818 100644 --- a/build.gradle +++ b/build.gradle @@ -53,6 +53,8 @@ def includeDesktopSupport = true dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + + implementation 'com.google.code.gson:gson:2.9.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/deploy/autos/auto-test.json b/src/main/deploy/autos/auto-test.json new file mode 100644 index 0000000..9fd15b2 --- /dev/null +++ b/src/main/deploy/autos/auto-test.json @@ -0,0 +1,26 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "SECONDS", + "amount": 0.1, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "wait", + "amount": 1 + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": -90, + "args": [ + 0.1, + -0.1 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/autonomous.json b/src/main/deploy/autos/autonomous.json new file mode 100644 index 0000000..4b841c6 --- /dev/null +++ b/src/main/deploy/autos/autonomous.json @@ -0,0 +1,35 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "CURRENT", + "amount": 35, + "args": [ + 0.1, + 0.1 + ] + }, + { + "type": "wait", + "amount": 0.5 + }, + { + "type": "shoot", + "unit": "SPEED", + "amount": 2 + }, + { + "type": "wait", + "amount": 0.5 + }, + { + "type": "drive", + "unit": "FEET", + "amount": 6.75, + "args": [ + -0.1, + -0.1 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/current-test.json b/src/main/deploy/autos/current-test.json new file mode 100644 index 0000000..ec52242 --- /dev/null +++ b/src/main/deploy/autos/current-test.json @@ -0,0 +1,26 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "CURRENT", + "amount": 20, + "args": [ + 0.05, + 0.05 + ] + }, + { + "type": "wait", + "amount": 1 + }, + { + "type": "drive", + "unit": "FEET", + "amount": 5, + "args": [ + -0.05, + -0.05 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/distance-test.json b/src/main/deploy/autos/distance-test.json new file mode 100644 index 0000000..01bd8fa --- /dev/null +++ b/src/main/deploy/autos/distance-test.json @@ -0,0 +1,76 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/shooter-test.json b/src/main/deploy/autos/shooter-test.json new file mode 100644 index 0000000..cceb488 --- /dev/null +++ b/src/main/deploy/autos/shooter-test.json @@ -0,0 +1,13 @@ +{ + "auto": [ + { + "type": "shoot", + "unit": "SPEED", + "amount": 1000 + }, + { + "type": "wait", + "amount": 5 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autonomous.java b/src/main/java/frc/robot/Autonomous.java new file mode 100644 index 0000000..2c45964 --- /dev/null +++ b/src/main/java/frc/robot/Autonomous.java @@ -0,0 +1,5 @@ +package frc.robot; + +public abstract class Autonomous { + public abstract void run(); +} diff --git a/src/main/java/frc/robot/DriveModule.java b/src/main/java/frc/robot/DriveModule.java index a422020..2d0a5e9 100644 --- a/src/main/java/frc/robot/DriveModule.java +++ b/src/main/java/frc/robot/DriveModule.java @@ -87,6 +87,10 @@ public void setSpeed(double speed) { main.set(TalonFXControlMode.Velocity, speed * 22000); } + public void setPower(double power) { + main.set(TalonFXControlMode.PercentOutput, power); + } + /** * Get the velocity of the module. * @@ -96,6 +100,15 @@ public double getSpeed() { return main.getSelectedSensorVelocity();// * VELOCITY_COEFFICIENT; } + /** + * Get the main sensor position (in raw sensor units). + * + * @return Position of selected sensor (in raw sensor units). + */ + public double getDriveEnc() { + return main.getSelectedSensorPosition(); + } + /** * Gets the average current drawn. * diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index e284eb3..0959338 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -42,9 +42,15 @@ public void update() { * @param leftSpeed The speed of the left motors * @param rightSpeed The speed of the right motors */ - public void drive(double leftSpeed, double rightSpeed) { // Probably implement deadbands later - left.set(leftSpeed); - right.set(rightSpeed); + public void drive(double leftValue, double rightValue, boolean power) { // Probably implement + // deadbands later + if (power) { + left.setPower(leftValue); + right.setPower(rightValue); + } else { + left.setSpeed(leftValue); + right.setSpeed(rightValue); + } } /** @@ -54,7 +60,7 @@ public void drive(double leftSpeed, double rightSpeed) { // Probably implement d * @param speedInput The speed to drive */ public void arcadeDrive(double turnInput, double speedInput) { - this.drive(speedInput - turnInput, speedInput + turnInput); + this.drive(speedInput - turnInput, speedInput + turnInput, false); } /** @@ -64,7 +70,7 @@ public void arcadeDrive(double turnInput, double speedInput) { * @param rightDrive The speed to set the right motors */ public void tankDrive(double leftDrive, double rightDrive) { - this.drive(leftDrive, rightDrive); + this.drive(leftDrive, rightDrive, false); } /** @@ -85,10 +91,26 @@ public boolean getShifter() { return shifter.get(); } + /** + * Gets the encoder information for the left DriveModule + * + * @return Position of left sensor (in raw sensor units). + */ + public double getEncoder() { + return (left.getDriveEnc() + right.getDriveEnc()) / 2; + } + + public double getAverageCurrent() { + return (left.getAverageCurrent() + right.getAverageCurrent()) / 2; + } + /** * Shifts gears based on current. */ public void checkGears() { + left.updateCurrent(); + right.updateCurrent(); + if (getShifter()) { if (left.getAverageCurrent() > SHIFT_CURRENT_LOW || right.getAverageCurrent() > SHIFT_CURRENT_LOW) { @@ -120,12 +142,14 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - // logger.addLoggable(left); - // logger.addLoggable(right); + // TODO Auto-generated method stub + } @Override public void log(Logger logger) { // TODO Auto-generated method stub + } + } diff --git a/src/main/java/frc/robot/JsonAutonomous.java b/src/main/java/frc/robot/JsonAutonomous.java new file mode 100644 index 0000000..b95332c --- /dev/null +++ b/src/main/java/frc/robot/JsonAutonomous.java @@ -0,0 +1,315 @@ +package frc.robot; + +import com.google.gson.JsonElement; +import com.google.gson.JsonIOException; +import com.google.gson.JsonObject; +import com.google.gson.JsonParser; +import com.google.gson.JsonSyntaxException; +import com.google.gson.stream.JsonReader; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.logging.Loggable; +import frc.robot.logging.LoggableGyro; +import frc.robot.logging.LoggableTimer; +import frc.robot.logging.Logger; + +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.util.ArrayList; +import java.util.List; +import java.util.NoSuchElementException; + +public class JsonAutonomous extends Autonomous implements Loggable { + + private static final double TICKS_PER_ROTATION = 16750; // TODO: Update value for 2022 robot + private static final double TICKS_PER_INCH = TICKS_PER_ROTATION / (6 * Math.PI); // TODO: Update + // formula for + // 2022 robot + private static final double SHOOTER_SPEED = 1; + private JsonElement auto; + private List instructions; + private int step; + private LoggableTimer timer; + private double start; + private double navxStart; + private LoggableGyro gyro; + + private Drivetrain drive; + + private Shooter shooter; + private Manipulation manipulation; + + private FileReader fr; + private JsonReader jr; + + public static class AutoInstruction { + public String type; + public Unit unit; + public Double amount; + public List args; + + public enum Unit { + SECONDS, MILLISECONDS, ENCODER_TICKS, ROTATIONS, INCHES, FEET, CURRENT, DEGREES, SPEED, POWER, INVALID + } + + public AutoInstruction(String type, List args) { + this.type = type; + this.args = args; + } + + public AutoInstruction(String type, Unit unit, Double amount, List args) { + this.type = type; + this.unit = unit; + this.amount = amount; + this.args = args; + } + } + + /** + * Creates a JsonAutonomous from the specified file + * + * @param file The location of the file to parse + */ + public JsonAutonomous(String file, LoggableGyro gyro, Drivetrain drive, Shooter shooter, + Manipulation manipulation) { + this.drive = drive; + this.gyro = gyro; + this.shooter = shooter; + this.manipulation = manipulation; + + parseFile(file); + } + + public void parseFile(String file) { + step = -1; + timer = new LoggableTimer(); + instructions = new ArrayList(); + try { + fr = new FileReader(file); + jr = new JsonReader(fr); + auto = JsonParser.parseReader(jr); + JsonElement inner = auto.getAsJsonObject().get("auto"); + if (inner.isJsonArray()) { + for (JsonElement e : inner.getAsJsonArray()) { + JsonObject o = e.getAsJsonObject(); + + List extraArgs = new ArrayList(); + if (o.has("args")) { + for (JsonElement e2 : o.get("args").getAsJsonArray()) { + extraArgs.add(e2.getAsDouble()); + } + } + + if (!o.has("type")) { + throw new NoSuchElementException( + "There is no element \"type\" in element " + e.toString() + "!"); + } + String type = o.get("type").getAsString(); + + String unitString = o.has("unit") ? o.get("unit").getAsString() : null; + AutoInstruction.Unit unit = unitString != null ? parseUnit(unitString) : null; + + Double amount = o.has("amount") ? o.get("amount").getAsDouble() : null; + + AutoInstruction ai = unit == null ? new AutoInstruction(type, extraArgs) + : new AutoInstruction(type, unit, amount, extraArgs); + instructions.add(ai); + } + } + } catch (JsonIOException | JsonSyntaxException | FileNotFoundException e) { + e.printStackTrace(); + } + } + + public static AutoInstruction.Unit parseUnit(String in) { + return AutoInstruction.Unit.valueOf(in); + } + + @Override + public void run() { + if (step == -1) { + reset(); + } + if (instructions.size() == step) { + drive.drive(0, 0, false); + return; + } + AutoInstruction ai = instructions.get(step); + + switch (ai.type) { + case "drive": + drive(ai); + break; + + case "turnDeg": + turnDegrees(ai); + break; + + case "wait": + wait(ai); + break; + + case "shoot": + shoot(ai); + break; + + default: + System.out.println("Invalid Command"); + reset(); + break; + } + } + + private void drive(AutoInstruction ai) { + AutoInstruction.Unit u = ai.unit; + // ai args: + // 0: leftPower + // 1: rightPower + if (u.equals(AutoInstruction.Unit.SECONDS) || u.equals(AutoInstruction.Unit.MILLISECONDS)) { + // amount: (milli)seconds to drive + if (driveTime(ai.args.get(0), ai.args.get(1), + (u.equals(AutoInstruction.Unit.SECONDS) ? ai.amount : ai.amount / 1000.0))) { + reset(); + } + } else if (u.equals(AutoInstruction.Unit.ENCODER_TICKS) + || u.equals(AutoInstruction.Unit.ROTATIONS)) { + // amount: rotations/encoder ticks to drive + if (driveDistance(ai.args.get(0), ai.args.get(1), + (u.equals(AutoInstruction.Unit.ENCODER_TICKS) ? ai.amount + : ai.amount * TICKS_PER_ROTATION))) { + reset(); + } + } else if (u.equals(AutoInstruction.Unit.FEET) || u.equals(AutoInstruction.Unit.INCHES)) { + // amount: feet/inches to drive + if (driveDistance(ai.args.get(0), ai.args.get(0), + (u.equals(AutoInstruction.Unit.INCHES) ? ai.amount * TICKS_PER_INCH + : (ai.amount * TICKS_PER_INCH) * 12))) { + reset(); + } + } else if (u.equals(AutoInstruction.Unit.CURRENT)) { + // amount: motor current to stop at + if (driveCurrent(ai.args.get(0), ai.args.get(0), ai.amount)) { + reset(); + } + } + } + + private boolean driveDistance(double leftPower, double rightPower, double distance) { + if (Math.abs(drive.getEncoder() - start) < distance) { + drive.drive(leftPower, rightPower, false); + } else { + return true; + } + return false; + } + + private boolean driveTime(double leftPower, double rightPower, double time) { + if (timer.get() < time) { + drive.drive(leftPower, rightPower, false); + } else { + return true; + } + return false; + } + + public boolean driveCurrent(double leftPower, double rightPower, double current) { + if (drive.getAverageCurrent() < current) { + drive.drive(leftPower, rightPower, false); + } else { + // drive.drive(0, 0); + return true; + } + return false; + } + + private boolean rotateDegrees(double leftSpeed, double rightSpeed, double deg) { + if (Math.abs(getAngle() - navxStart - deg) < 10) { + return true; + } else { + drive.drive(leftSpeed, rightSpeed, false); + return false; + } + } + + public void turnDegrees(AutoInstruction ai) { + // ai args: + // 0: leftPower + // 1: rightPower + // amount: degrees to turn + if (rotateDegrees(ai.args.get(0), ai.args.get(1), ai.amount)) { + drive.drive(0, 0, false); // Stop turning + reset(); + } + } + + private void wait(AutoInstruction ai) { + if (timer.get() >= ai.amount) { + reset(); + } + } + + private void shoot(AutoInstruction ai) { + AutoInstruction.Unit u = ai.unit; + + if (u == AutoInstruction.Unit.SPEED) { + if (shooter.getSpeed() < SHOOTER_SPEED) { + shooter.setSpeed(SHOOTER_SPEED); + } else { + timer.reset(); + if (timer.get() < ai.args.get(0)) { + shooter.setSpeed(SHOOTER_SPEED); + manipulation.setIntakeSpin(true); + } else { + shooter.setSpeed(0); + manipulation.setIntakeSpin(false); + reset(); + } + } + } else if (u == AutoInstruction.Unit.POWER) { + if (shooter.getSpeed() < SHOOTER_SPEED) { + shooter.setPower(SHOOTER_SPEED); + } else { + timer.reset(); + if (timer.get() < ai.args.get(0)) { + shooter.setPower(SHOOTER_SPEED); + manipulation.setIntakeSpin(true); + } else { + shooter.setPower(0); + manipulation.setIntakeSpin(false); + reset(); + } + } + } + + } + + private void reset() { + drive.drive(0, 0, true); + step++; + navxStart = getAngle(); + start = drive.getEncoder(); + timer.reset(); + timer.start(); + } + + private double getAngle() { + return gyro.getAngle(); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Autonomous/step"); + } + + @Override + public void log(Logger logger) { + logger.log("Autonomous/step", this.step); + } + + public static String getAutoPath(String name) { + return Filesystem.getDeployDirectory().listFiles((dir, filename) -> { + return filename.contentEquals("autos"); + })[0].listFiles((dir, filename) -> { + return filename.contentEquals(name); + })[0].getAbsolutePath(); + } +} diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java new file mode 100644 index 0000000..e1cd165 --- /dev/null +++ b/src/main/java/frc/robot/Manipulation.java @@ -0,0 +1,82 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Manipulation implements Loggable { + + private CANSparkMax intakeWheel; + private DoubleSolenoid intakePneumatics; + private CANSparkMax indexLoad; + + private double speed; + private boolean spinning; + + /** + * Constructor + * + * @param pneumaticsForwardChannel The Solenoid id for the forward channel for the intake + * @param pneumaticsReverseChannel The Solenoid id for the reverse channel for the intake + * @param intakeWheelID The CAN id of the spark for the intake + * @param indexLoadID The CAN id of the spark for the index loader + * + */ + Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID, int indexLoadID) { + this.intakeWheel = new CANSparkMax(intakeWheelID, MotorType.kBrushless); + this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); + this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, pneumaticsForwardChannel, pneumaticsReverseChannel); + + intakeWheel.setInverted(true); + } + + /** + * Spins the intake motor + * + * @param spin True if the motor should spin; false if not + * + */ + public void setIntakeSpin(boolean spin) { + this.speed = spin ? -0.5 : 0.0; + intakeWheel.set(speed); + this.spinning = spin; + } + + /** + * Moves the intake system + * + * @param extend True if the pneumatics should extend; false if not + * + */ + public void setIntakeExtend(boolean extend) { + intakePneumatics.set(extend ? Value.kForward : Value.kReverse); + } + /** + * Moves power cells down indexing system + * + * @param load True if it should load; false if not + * + */ + public void setIndexLoad(boolean load) { + indexLoad.set(load ? 0.5 : 0.0); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Manipulation/IntakeWheelSpeed"); + logger.addAttribute("Manipulation/IntakeWheelEnabled"); + } + + @Override + public void log(Logger logger) { + logger.log("Manipulation/IntakeWheelSpeed", speed); + logger.log("Manipulation/IntakeWheelEnabled", spinning); + } + +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 83ff985..c2d357a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,14 +36,22 @@ public class Robot extends TimedRobot { LoggableController driver; LoggableController operator; + Manipulation manipulation; + Shooter shooter; LoggablePowerDistribution pdp; LoggableCompressor compressor; boolean drivetrainEnabled = true; boolean tankDriveEnabled = true; + boolean manipulationEnabled = true; + boolean shooterEnabled = true; boolean climberEnabled = true; + private JsonAutonomous auto; + + LoggableGyro gyro; + private static final double DEADBAND_LIMIT = 0.01; private static final double SPEED_CAP = 0.3; InputScaler joystickDeadband = new Deadband(DEADBAND_LIMIT); @@ -74,10 +82,10 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); - Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); - Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); - Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); - Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); + Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 1); + Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 2); + Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 3); + Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 4); // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); @@ -90,20 +98,37 @@ public void robotInit() { System.out.println("Climber initialization disabled."); } + gyro = new LoggableGyro(); + gyro.enableLogging(false); + if (this.drivetrainEnabled) { System.out.println("Initializing drivetrain..."); - leftModule = new DriveModule("LeftDriveModule", 2, 3); // 2, 3 + leftModule = new DriveModule("LeftDriveModule", 2, 3); leftModule.setEncoder(2, 3, false); - rightModule = new DriveModule("RightDriveModule", 4, 5); // 4, 5 + rightModule = new DriveModule("RightDriveModule", 4, 5); rightModule.setEncoder(0, 1, true); - drive = new Drivetrain(leftModule, rightModule, 6); + drive = new Drivetrain(leftModule, rightModule, 0); logger.addLoggable(drive); } else { System.out.println("Drivetrain initialization disabled."); } + if (this.manipulationEnabled) { + System.out.println("Initializing manipulation..."); + manipulation = new Manipulation(5, 6, 7, 8); + } else { + System.out.println("Manipulation initialization disabled."); + } + if (this.shooterEnabled) { + System.out.println("Initializing shooter"); + shooter = new Shooter(6); + logger.addLoggable(shooter); + System.out.println("Shooter done"); + } else { + System.out.println("Shooter initialization disabled."); + } System.out.print("Initializing compressor..."); compressor = new LoggableCompressor(PneumaticsModuleType.REVPH); @@ -123,12 +148,21 @@ public void robotPeriodic() { @Override public void autonomousInit() { + gyro.reset(); + + auto = new JsonAutonomous(JsonAutonomous.getAutoPath("shooter-test.json"), gyro, drive, shooter, manipulation); + System.out.println("Auto Initialized"); + logger.addLoggable(auto); resetLogging(); } @Override public void autonomousPeriodic() { // Robot code goes here + leftModule.updateCurrent(); + rightModule.updateCurrent(); + auto.run(); + logger.log(); logger.writeLine(); } @@ -162,6 +196,18 @@ public void teleopPeriodic() { leftModule.updateCurrent(); rightModule.updateCurrent(); } + if (this.manipulationEnabled) { + if (driver.getRightBumperPressed()) { + manipulation.setIntakeExtend(true); + } else if (driver.getLeftBumperPressed()) { + manipulation.setIntakeExtend(false); + } + manipulation.setIntakeSpin(operator.getYButton()); + manipulation.setIndexLoad(operator.getXButton()); + } + if (this.shooterEnabled) { + shooter.setSpeed(driver.getRightTriggerAxis()); + } if (this.climberEnabled) { double climberInput = deadband(operator.getLeftY()); @@ -171,13 +217,11 @@ public void teleopPeriodic() { } // climber.checkClimbingState(operator.getAButtonPressed()); - // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro if (operator.getRightBumperPressed()) { climber.setMotorState( climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC : MotorStates.ACTIVE); } - // TODO: Enable this when we're ready to test the climber } logger.log(); diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java new file mode 100644 index 0000000..27ceb04 --- /dev/null +++ b/src/main/java/frc/robot/Shooter.java @@ -0,0 +1,38 @@ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Shooter implements Loggable { + TalonFX shooterMotor; + Shooter(int shooterID) { + this.shooterMotor = new TalonFX(shooterID); + + this.shooterMotor.setNeutralMode(NeutralMode.Coast); + } + + public void setSpeed(double speed) { + shooterMotor.set(ControlMode.Velocity, speed); + } + + public void setPower(double power) { + shooterMotor.set(ControlMode.PercentOutput, power); + } + + public double getSpeed() { + return shooterMotor.getSelectedSensorVelocity(); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Shooter/speed"); + } + + @Override + public void log(Logger logger) { + logger.log("Shooter/speed", this.getSpeed()); + } +} diff --git a/src/main/java/frc/robot/logging/LoggableGyro.java b/src/main/java/frc/robot/logging/LoggableGyro.java index bb7083d..cb67840 100644 --- a/src/main/java/frc/robot/logging/LoggableGyro.java +++ b/src/main/java/frc/robot/logging/LoggableGyro.java @@ -9,27 +9,15 @@ public LoggableGyro() { @Override public void setupLogging(Logger logger) { - logger.addAttribute("AHRS/velocityX"); - logger.addAttribute("AHRS/yaw"); - logger.addAttribute("AHRS/accelerationX"); logger.addAttribute("AHRS/velocityY"); logger.addAttribute("AHRS/pitch"); logger.addAttribute("AHRS/accelerationY"); - logger.addAttribute("AHRS/velocityZ"); - logger.addAttribute("AHRS/roll"); - logger.addAttribute("AHRS/accelerationZ"); } @Override public void log(Logger logger) { - logger.log("AHRS/velocityX", this.getVelocityX()); - logger.log("AHRS/yaw", this.getYaw()); - logger.log("AHRS/accelerationX", this.getWorldLinearAccelX()); logger.log("AHRS/velocityY", this.getVelocityY()); logger.log("AHRS/pitch", this.getPitch()); logger.log("AHRS/accelerationY", this.getWorldLinearAccelY()); - logger.log("AHRS/velocityZ", this.getVelocityZ()); - logger.log("AHRS/roll", this.getRoll()); - logger.log("AHRS/accelerationZ", this.getWorldLinearAccelZ()); } } diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..997e2a4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file