diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e7634d1..0420079 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -139,6 +139,16 @@ public AnglerConstants( } } + public static class ElevatorConstants { + public static final double maxHeight = 4; // Temp + public static final double minHeight = 0; + + public static final MotionMagic pid = new MotionMagic(.05, 0, .05, 0); // temp + + public static final double roboWeight = 120; // Temp + public static final boolean enableFF = true; + } + public static class DrivebaseConstant { public static final int ID_PIGEON2 = 13; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 27b46d2..f69b767 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.commands.RunManipulatorCommand; import frc.robot.subsystems.Angler; import frc.robot.subsystems.Drivebase; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Manipulator; import frc.robot.subsystems.PheonixDrivebase; @@ -35,7 +36,8 @@ public class RobotContainer { private final Angler pivot = systems.getPivot(); private final Intake intake = systems.getIntake(); private final Shooter shooter = systems.getShooter(); - + private final Elevator elevator = systems.getElevator(); + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() //.withDeadband(TunerConstatns.kSpeedAt12VoltsMps * 0.2).withRotationalDeadband(TunerConstatns.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); diff --git a/src/main/java/frc/robot/Systems.java b/src/main/java/frc/robot/Systems.java index d144e9e..784871c 100644 --- a/src/main/java/frc/robot/Systems.java +++ b/src/main/java/frc/robot/Systems.java @@ -10,6 +10,7 @@ import frc.robot.Constants.TunerConstatns; import frc.robot.subsystems.Angler; import frc.robot.subsystems.Drivebase; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.PheonixDrivebase; import frc.robot.subsystems.Shooter; @@ -23,12 +24,15 @@ public class Systems { private Angler shooterAngler; private Drivebase drivebase; private Shooter shooter; - + private Elevator elevator; private AnglerConstants intakeAnglerConst; private CANSparkFlex shooterUpper; private CANSparkFlex shooterLower; + private CANSparkFlex elevatorLeft; + private CANSparkFlex elevatorRight; + private CANSparkMax leftIntakeMotor; private CANSparkMax rightIntakeMotor; private CANSparkMax intakeAnglerMotor; @@ -61,6 +65,8 @@ public Systems() { intake = new Intake(leftIntakeMotor, rightIntakeMotor); pivot = new Angler(intakeAnglerMotor, intakeAnglerConst, "pivot"); + + // elevator = new Elevator(elevatorLeft, elevatorRight); } public PheonixDrivebase getDrivebase() { @@ -86,4 +92,8 @@ public Angler getPivot() { public Angler getShooterAngler() { return shooterAngler; } + + public Elevator getElevator() { + return elevator; + } } diff --git a/src/main/java/frc/robot/commands/RunElevatorCommand.java b/src/main/java/frc/robot/commands/RunElevatorCommand.java new file mode 100644 index 0000000..7737760 --- /dev/null +++ b/src/main/java/frc/robot/commands/RunElevatorCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; + +public class RunElevatorCommand extends Command{ + private Elevator elevator; + + public RunElevatorCommand (Elevator elevator) { + this.elevator = elevator; + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + elevator.setPosition(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..b7d342a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ElevatorConstants; + +public class Elevator extends SubsystemBase { + + private CANSparkFlex left; + private CANSparkFlex right; + private RelativeEncoder relativeEncoder; + public SparkPIDController controller; + + public Elevator (CANSparkFlex left, CANSparkFlex right) { + this.left = left; + this.right = right; + this.controller = right.getPIDController(); + this.controller.setP(ElevatorConstants.pid.p()); + this.controller.setI(ElevatorConstants.pid.i()); + this.controller.setD(ElevatorConstants.pid.d()); + + left.setInverted(true); // dunno if needed + right.setInverted(true); // dunno if needed + left.follow(right); + controller.setFeedbackDevice(relativeEncoder); + + this.left.burnFlash(); + this.right.burnFlash(); + } + + public void setPosition () { + double downForce = Units.lbsToKilograms(ElevatorConstants.roboWeight) * 9.81; + + controller.setReference( + ElevatorConstants.maxHeight, + CANSparkBase.ControlType.kPosition, + 0, + ElevatorConstants.enableFF ? downForce : 0, + ArbFFUnits.kPercentOut + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 19b9f6e..5b9a37f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,17 +8,17 @@ public class Intake extends Manipulator { - CANSparkBase motor; - CANSparkBase kingBob; // as in the rubber duck not the stupid filet minion + CANSparkBase duckPrime; // the second duck! + CANSparkBase kingBob; // as in the rubber duck not the stupid filet minion - public Intake(CANSparkBase motor, CANSparkBase kingBob) { - this.motor = motor; + public Intake(CANSparkBase duckPrime, CANSparkBase kingBob) { + this.duckPrime = duckPrime; this.kingBob = kingBob; - //kingBob.follow(motor); + //kingBob.follow(duckPrime); kingBob.setInverted(false); kingBob.burnFlash(); - motor.setInverted(false); - motor.burnFlash(); + duckPrime.setInverted(false); + duckPrime.burnFlash(); this.setName("Intake"); } @@ -35,17 +35,17 @@ public double getReverseVelocityMultiplier() { @Override public void runNeutral() { - motor.setIdleMode(IdleMode.kCoast); + duckPrime.setIdleMode(IdleMode.kCoast); kingBob.setIdleMode(IdleMode.kCoast); - motor.burnFlash(); + duckPrime.burnFlash(); kingBob.burnFlash(); } @Override public void stopNeutral() { - motor.setIdleMode(IdleMode.kBrake); + duckPrime.setIdleMode(IdleMode.kBrake); kingBob.setIdleMode(IdleMode.kCoast); - motor.burnFlash(); + duckPrime.burnFlash(); kingBob.burnFlash(); } @@ -53,12 +53,12 @@ public void stopNeutral() { public void runWithPower(double power) { super.runWithPower(power); kingBob.set(power); - motor.set(power); + duckPrime.set(power); } @Override public void periodic() { - SmartDashboard.putNumber("Intake currentOut", motor.getOutputCurrent()); + SmartDashboard.putNumber("Intake currentOut", duckPrime.getOutputCurrent()); SmartDashboard.putBoolean("Has Gamepice", checkGamePieceStatus()); } @@ -69,9 +69,9 @@ public void periodic() { */ @Override public boolean checkGamePieceStatus() { - if(motor.getOutputCurrent() > 30) { + if(duckPrime.getOutputCurrent() > 30) { hadGamePiece = true; - }else if(motor.get() < 0) { + }else if(duckPrime.get() < 0) { hadGamePiece = false; }