diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd11f1e3..0e3f76f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -176,11 +176,7 @@ public class FlywheelConstants { public static final double FLYWHEEL_MOTOR_KS = 0; public static final double FLYWHEEL_MOTOR_KV = 0; - public static final double FLYWHEEL_RPM_TOLERANCE = 0; - // Distance in meters, angle in degrees - public static final InterpolationMap SHOOTER_DISTANCE_ANGLE_MAP = new InterpolationMap(); - // Distance in meters, speed in RPM - public static final InterpolationMap SHOOTER_DISTANCE_SPEED_MAP = new InterpolationMap(); + public static final double RPM_TOLERANCE = 0; } public class PivotConstants { @@ -193,7 +189,31 @@ public class PivotConstants { public static final double PIVOT_MOTOR_KD = 0; public static final double PIVOT_MOTOR_KS = 0; public static final double PIVOT_MOTOR_KV = 0; - public static final double PIVOT_TOLERANCE = 0; + public static final double ANGLE_TOLERANCE = 0; + + } + + public class ShooterConstants { + public static final double BASE_RPM = 0; + public static final double STOW_ANGLE = 0; + + + public enum SHOOTER_STATES { + STOW, + PRIME, + AIM, + SHOOT + } + + // Distance in meters, angle in degrees + public static final InterpolationMap ANGLE_MAP = new InterpolationMap() {{ + put(0d, 0d); + }}; + + // Distance in meters, speed in RPM + public static final InterpolationMap SPEED_MAP = new InterpolationMap() {{ + put(0d, 0d); + }}; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c9557fe..a5ca09ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.TunerConstants; import frc.robot.Constants.drivetrainConstants; -import frc.robot.command.Shoot; import frc.robot.subsystems.Swerve; import frc.thunder.LightningContainer; diff --git a/src/main/java/frc/robot/command/Shoot.java b/src/main/java/frc/robot/command/Shoot.java deleted file mode 100644 index fcc94e65..00000000 --- a/src/main/java/frc/robot/command/Shoot.java +++ /dev/null @@ -1,44 +0,0 @@ -package frc.robot.command; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Flywheel; -import frc.robot.subsystems.Pivot; -import frc.robot.subsystems.ShooterTargeting; - -public class Shoot extends Command { - Flywheel flywheel; - Pivot pivot; - ShooterTargeting shooterTargeting; - - public Shoot(Flywheel flywheel, Pivot pivot, ShooterTargeting shooterTargeting) { - this.flywheel = flywheel; - this.pivot = pivot; - this.shooterTargeting = shooterTargeting; - - addRequirements(flywheel, pivot, shooterTargeting); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - flywheel.setRPM(shooterTargeting.getTargetFlywheelRPM()); - pivot.setAngle(shooterTargeting.getTargetFlywheelAngle()); - } - - @Override - public void execute() { - if(shooterTargeting.readyToFire()); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - flywheel.setRPM(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index eec5d600..428fc2c3 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -12,7 +12,9 @@ public class Flywheel extends SubsystemBase { private TalonFX shooterMotor1; private TalonFX shooterMotor2; - final VelocityVoltage rpmTarget = new VelocityVoltage(0).withSlot(0); + private double targetRPM = 0; + + private final VelocityVoltage rpmPID = new VelocityVoltage(0).withSlot(0); public Flywheel() { shooterMotor1 = FalconConfig.createMotor(CAN.FLYWHEEL_MOTOR_1, getName(), FlywheelConstants.FLYWHEEL_MOTOR_1_INVERT, FlywheelConstants.FLYWHEEL_MOTOR_SUPPLY_CURRENT_LIMIT, FlywheelConstants.FLYWHEEL_MOTOR_STATOR_CURRENT_LIMIT, FlywheelConstants.FLYWHEEL_MOTOR_NEUTRAL_MODE, FlywheelConstants.FLYWHEEL_MOTOR_KP, FlywheelConstants.FLYWHEEL_MOTOR_KI, FlywheelConstants.FLYWHEEL_MOTOR_KD, FlywheelConstants.FLYWHEEL_MOTOR_KS, FlywheelConstants.FLYWHEEL_MOTOR_KV); @@ -24,8 +26,10 @@ public Flywheel() { * @param rpm RPM of the flywheel */ public void setRPM(double rpm){ - shooterMotor1.setControl(rpmTarget.withVelocity(rpm)); - shooterMotor2.setControl(rpmTarget.withVelocity(rpm)); + targetRPM = rpm; + + shooterMotor1.setControl(rpmPID.withVelocity(rpm)); + shooterMotor2.setControl(rpmPID.withVelocity(rpm)); } /** @@ -34,4 +38,11 @@ public void setRPM(double rpm){ public double getFlywheelRPM() { return (shooterMotor1.getVelocity().getValue() + shooterMotor2.getVelocity().getValue()) / 2; } + + /** + * @return Whether or not the flywheel is on target, within FlywheelConstants.RPM_TOLERANCE + */ + public boolean onTarget() { + return Math.abs(getFlywheelRPM() - targetRPM) < FlywheelConstants.RPM_TOLERANCE; + } } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 7cc67277..e499b987 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -10,8 +10,10 @@ public class Pivot extends SubsystemBase { private TalonFX angleMotor; - - final PositionVoltage angleTarget = new PositionVoltage(0).withSlot(0); + private final PositionVoltage anglePID = new PositionVoltage(0).withSlot(0); + + //TODO: find initial target angle + private double targetAngle = 0; public Pivot() { angleMotor = FalconConfig.createMotor(CAN.FLYWHEEL_MOTOR_1, getName(), PivotConstants.PIVOT_MOTOR_INVERT, PivotConstants.PIVOT_MOTOR_SUPPLY_CURRENT_LIMIT, PivotConstants.PIVOT_MOTOR_STATOR_CURRENT_LIMIT, PivotConstants.PIVOT_MOTOR_NEUTRAL_MODE, PivotConstants.PIVOT_MOTOR_KP, PivotConstants.PIVOT_MOTOR_KI, PivotConstants.PIVOT_MOTOR_KD, PivotConstants.PIVOT_MOTOR_KS, PivotConstants.PIVOT_MOTOR_KV); @@ -22,7 +24,8 @@ public Pivot() { * @param angle Angle of the pivot */ public void setAngle(double angle) { - angleMotor.setControl(angleTarget.withPosition(angle)); + targetAngle = angle; + angleMotor.setControl(anglePID.withPosition(angle)); } /** @@ -31,4 +34,11 @@ public void setAngle(double angle) { public double getAngle() { return angleMotor.getPosition().getValue(); } + + /** + * @return Whether or not the pivot is on target, within PivotConstants.ANGLE_TOLERANCE + */ + public boolean onTarget() { + return Math.abs(getAngle() - targetAngle) < PivotConstants.ANGLE_TOLERANCE; + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 00000000..b358a239 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterConstants.SHOOTER_STATES; + +public class Shooter extends SubsystemBase { + private Pivot pivot; + private Flywheel flywheel; + //TODO: add collector/indexer + + //TODO: find initial target angle + private double currentDistance = 0; + + private SHOOTER_STATES state = SHOOTER_STATES.STOW; + + public Shooter(Pivot pivot, Flywheel flywheel) { + this.pivot = pivot; + this.flywheel = flywheel; + } + + @Override + public void periodic() { + //TODO: IMPLEMENT VISION/ODO + currentDistance = 0; + switch (state) { + //Stow pivot to allow collector to input not\ + case STOW: + pivot.setAngle(ShooterConstants.STOW_ANGLE); + flywheel.setRPM(0); + break; + //Prime is warming up the flywheel for shooting + case PRIME: + pivot.setAngle(ShooterConstants.STOW_ANGLE); + flywheel.setRPM(ShooterConstants.BASE_RPM); + break; + //Aim is targeting the pivot toward the target and speeding up the flywheel + case AIM: + pivot.setAngle(ShooterConstants.ANGLE_MAP.get(currentDistance)); + flywheel.setRPM(ShooterConstants.SPEED_MAP.get(currentDistance)); + break; + // Shoot is shooting the note + case SHOOT: + pivot.setAngle(ShooterConstants.ANGLE_MAP.get(currentDistance)); + flywheel.setRPM(ShooterConstants.SPEED_MAP.get(currentDistance)); + + //If flywheel and pivot are on target (ready to shoot), shoot + if(flywheel.onTarget() && pivot.onTarget()) { + //index + } + break; + } + } + + public SHOOTER_STATES getState() { + return state; + +} + + /** + * Sets the state of the shooter (STOW, PRIME, AIM, SHOOT) + * note: + * @param state + */ + public void setState(SHOOTER_STATES state) { + this.state = state; + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterTargeting.java b/src/main/java/frc/robot/subsystems/ShooterTargeting.java deleted file mode 100644 index 6ebc5e49..00000000 --- a/src/main/java/frc/robot/subsystems/ShooterTargeting.java +++ /dev/null @@ -1,54 +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 edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.FlywheelConstants; - -public class ShooterTargeting extends SubsystemBase { - Flywheel shooter = new Flywheel(); - double targetFlywheelRPM; - double targetFlywheelAngle; - double distanceFromTarget; - public ShooterTargeting( - - ) {} - - @Override - public void periodic() { - // This method will be called once per scheduler run - distanceFromTarget = getDistanceFromTarget(); - targetFlywheelAngle = clacShooterAngel(); - targetFlywheelRPM = clacShooterRPM(); - } - - public boolean readyToFire() { - if (Math.abs(shooter.getFlywheelRPM() - targetFlywheelRPM) < FlywheelConstants.FLYWHEEL_RPM_TOLERANCE) { - return true; - } else { - return false; - } - } - - public double clacShooterRPM() { - return FlywheelConstants.SHOOTER_DISTANCE_SPEED_MAP.get(distanceFromTarget); - } - - public double clacShooterAngel() { - return FlywheelConstants.SHOOTER_DISTANCE_ANGLE_MAP.get(distanceFromTarget); - } - - public double getDistanceFromTarget() { - return 0; //TODO make work - } - - public double getTargetFlywheelRPM() { - return targetFlywheelRPM; - } - - public double getTargetFlywheelAngle() { - return targetFlywheelAngle; - } -} diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index fbb0f3f1..9a8336c6 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit fbb0f3f1307f60ca3ed23964530f2eca6079abe5 +Subproject commit 9a8336c695d20a52bbdea110c08ec2139c25718e