-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Initial commit w/ partial done controls
- Loading branch information
1 parent
a900ed2
commit 0cb15b2
Showing
7 changed files
with
659 additions
and
5 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
150 changes: 150 additions & 0 deletions
150
src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,150 @@ | ||
| // 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.revrobotics.spark.SparkBase.ControlType; | ||
| import com.revrobotics.spark.SparkBase.PersistMode; | ||
| import com.revrobotics.spark.SparkBase.ResetMode; | ||
| import com.revrobotics.spark.SparkClosedLoopController; | ||
| import com.revrobotics.spark.SparkLowLevel.MotorType; | ||
| import com.revrobotics.spark.SparkMax; | ||
| import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; | ||
| import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; | ||
| import com.revrobotics.spark.config.SparkMaxConfig; | ||
|
|
||
| import edu.wpi.first.math.filter.Debouncer; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants.AlgaeConstants; | ||
|
|
||
| public class AlgaeGrabberSubsystem extends SubsystemBase { | ||
| private final SparkMax m_flywheel; | ||
| private final SparkMax m_grabberAngleMotor = new SparkMax(AlgaeConstants.kGrabberAnglePort, | ||
| MotorType.kBrushless); | ||
| private final SparkClosedLoopController m_grabberClosedLoopController = m_grabberAngleMotor | ||
| .getClosedLoopController(); | ||
|
|
||
| Debouncer m_debouncerCurrentLimitStop; | ||
|
|
||
| private double m_setVelocity; | ||
|
|
||
| public enum grabberState { | ||
| UP, | ||
| DOWN | ||
| } | ||
|
|
||
| public AlgaeGrabberSubsystem() { | ||
| SparkMaxConfig config; | ||
| m_debouncerCurrentLimitStop = new Debouncer(AlgaeConstants.kTimeOverCurrentToStop); | ||
| m_flywheel = new SparkMax(AlgaeConstants.kFlywheelMotorPort, MotorType.kBrushless); | ||
|
|
||
| // Initialize Motors | ||
| // applies the inverts and coast mode to the flywheel motors. if you want to | ||
| // make a motor spin the other way change it in the AlgaeConstants.k____Invert | ||
| // variable | ||
| // also applies voltage and current stuff to the motors | ||
|
|
||
| config = new SparkMaxConfig(); | ||
| config.inverted(AlgaeConstants.kFlywheelInvert).idleMode(IdleMode.kBrake); | ||
| config.voltageCompensation(12).smartCurrentLimit(AlgaeConstants.k550SmartCurrentLimit); | ||
| m_flywheel.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); | ||
|
|
||
| config = new SparkMaxConfig(); | ||
| config.inverted(AlgaeConstants.kGrabberAngleInvert).idleMode(IdleMode.kBrake); | ||
| config.voltageCompensation(12).smartCurrentLimit(AlgaeConstants.kSmartCurrentLimit); | ||
| config.closedLoop | ||
| .feedbackSensor(FeedbackSensor.kPrimaryEncoder) | ||
| .pid(AlgaeConstants.kP, AlgaeConstants.kI, AlgaeConstants.kD); | ||
| m_grabberAngleMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| // use this to check what the kCurrentToStop should be | ||
| // System.out.println(m_flywheel.getOutputCurrent()); | ||
| } | ||
|
|
||
| public double getVelocity() { | ||
| return m_flywheel.getAbsoluteEncoder().getVelocity(); | ||
| } | ||
|
|
||
| /** | ||
| * sets the speed for the wheels to grab the Algae balls | ||
| * | ||
| * @param velocity (double) (-1 to 1) sets m_setVelocity and changes the wheel | ||
| * to go that speed using .set from {@link SparkMax} | ||
| * @return (void) motor will spin velocity | ||
| */ | ||
| public void setVelocity(double velocity) { | ||
| m_setVelocity = velocity; | ||
| m_flywheel.set(m_setVelocity); | ||
| } | ||
|
|
||
| /** | ||
| * checks the current draw on the flywheel motor and will check if it's lower or | ||
| * higher than AlgaeConstants.kCurrentToStop using a debouncer | ||
| * | ||
| * @return (boolean) true -> if the current draw is higher or equal than | ||
| * kCurrentToStop | ||
| * <p> | ||
| * (boolean) false -> otherwise | ||
| */ | ||
| public boolean checkCurrentOnFlywheel() { | ||
| return m_debouncerCurrentLimitStop | ||
| .calculate(m_flywheel.getOutputCurrent() >= AlgaeConstants.kCurrentToStop); | ||
| } | ||
|
|
||
| /** | ||
| * command to stop the algae flywheel using the setVelocity method | ||
| * | ||
| * @return (setVelocity(0)) sets the flywheel velocity to 0 | ||
| */ | ||
| public Command stopFlywheelCommand() { | ||
| return runOnce(() -> { | ||
| setVelocity(0); | ||
| }); | ||
| } | ||
|
|
||
| /** | ||
| * command to start the algae flywheel using the setVelocity method | ||
| * | ||
| * @return (setVelocity(.75)) sets the flywheel velocity to 75% | ||
| */ | ||
| public Command runFlywheelCommand() { | ||
| return runOnce(() -> { | ||
| setVelocity(.75); | ||
| }); | ||
| } | ||
|
|
||
| /** | ||
| * command to reverse the algae flywheel using the setVelocity method | ||
| * | ||
| * @return (setVelocity(-.75)) sets the flywheel velocity to -75% | ||
| */ | ||
| public Command runFlywheelReverseCommand() { | ||
| return runOnce(() -> { | ||
| setVelocity(-.75); | ||
| }); | ||
| } | ||
|
|
||
| /** | ||
| * deploys the grabber for algae | ||
| * | ||
| * @param state (enum grabberState) can be UP or DOWN, UP will bring the grabber | ||
| * up, DOWN will bring it down | ||
| * | ||
| * @return moves the whole grabber setup using a PID based on the grabberState | ||
| * enum given | ||
| */ | ||
| public Command deployGrabberCommand(grabberState state) { | ||
| return runOnce(() -> { | ||
| if (grabberState.DOWN == state) { | ||
| m_grabberClosedLoopController.setReference(2, ControlType.kPosition); | ||
| } else if (grabberState.UP == state) { | ||
| m_grabberClosedLoopController.setReference(0, ControlType.kPosition); | ||
| } | ||
| }); | ||
| } | ||
| } |
42 changes: 42 additions & 0 deletions
42
src/main/java/frc/robot/subsystems/CheeseStickSubsystem.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,42 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import static frc.robot.Constants.CheeseStickConstants.*; | ||
|
|
||
| import edu.wpi.first.wpilibj.Servo; | ||
| import edu.wpi.first.wpilibj.Timer; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
|
|
||
| public class CheeseStickSubsystem extends SubsystemBase { | ||
| private Servo m_motor; | ||
| public Timer time; | ||
|
|
||
| public CheeseStickSubsystem() { | ||
| m_motor = new Servo(kServoPort); | ||
| time = new Timer(); | ||
| m_motor.setBoundsMicroseconds(2500, 0, 1500, 0, 500); | ||
| } | ||
|
|
||
| public void init() { | ||
| m_motor.setZeroLatch(); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| SmartDashboard.putNumber("Time", time.get()); | ||
| SmartDashboard.putNumber("Position", m_motor.getPosition()); | ||
| } | ||
|
|
||
| /** | ||
| * Rotates the servo. Does not wait for it to finish. | ||
| * | ||
| * @param angle The angle from 0 in the left to 270 in the right. | ||
| * @return The command. | ||
| */ | ||
| public Command rotateCommand(Double angle) { | ||
| if (angle == null) | ||
| return runOnce(() -> m_motor.set(.5)); | ||
| return runOnce(() -> m_motor.set(angle / kMaxRotation)).withName("Rotate Servo"); | ||
| } | ||
| } |
Oops, something went wrong.