diff --git a/.idea/.gitignore b/.idea/.gitignore deleted file mode 100644 index 8f00030d..00000000 --- a/.idea/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml -# GitHub Copilot persisted chat sessions -/copilot/chatSessions diff --git a/.idea/Common-Library-And-Wiki.iml b/.idea/Common-Library-And-Wiki.iml deleted file mode 100644 index d6ebd480..00000000 --- a/.idea/Common-Library-And-Wiki.iml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml deleted file mode 100644 index 436f65c4..00000000 --- a/.idea/codeStyles/Project.xml +++ /dev/null @@ -1,303 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml deleted file mode 100644 index 79ee123c..00000000 --- a/.idea/codeStyles/codeStyleConfig.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/gradle.xml b/.idea/gradle.xml deleted file mode 100644 index 961bd4f7..00000000 --- a/.idea/gradle.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml deleted file mode 100644 index 0528db24..00000000 --- a/.idea/jarRepositories.xml +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml deleted file mode 100644 index 726dafd4..00000000 --- a/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 69a3492e..00000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/.idea/modules/RobotCode.iml b/.idea/modules/RobotCode.iml deleted file mode 100644 index 248b0035..00000000 --- a/.idea/modules/RobotCode.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml deleted file mode 100644 index 35eb1ddf..00000000 --- a/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 8aa48787..19c7b699 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -8,6 +8,6 @@ "editor.detectIndentation": false }, "editor.stickyTabStops": true, - "editor.formatOnSave": true, - "editor.formatOnPaste": true + "editor.formatOnSave": false, + "editor.formatOnPaste": false } \ No newline at end of file diff --git a/RobotCode/src/main/java/frc/lib/core/util/TeamMotorUtil.java b/RobotCode/src/main/java/frc/lib/core/util/TeamMotorUtil.java new file mode 100644 index 00000000..574c8041 --- /dev/null +++ b/RobotCode/src/main/java/frc/lib/core/util/TeamMotorUtil.java @@ -0,0 +1,20 @@ +package frc.lib.core.util; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; + +public class TeamMotorUtil { + + private static final int MAX_PERIODIC_FRAME_PERIOD = 65535; + + public static void optimizeCANSparkBusUsage(CANSparkBase motor) { + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, MAX_PERIODIC_FRAME_PERIOD); + motor.setPeriodicFramePeriod(PeriodicFrame.kStatus7, MAX_PERIODIC_FRAME_PERIOD); + } +} \ No newline at end of file diff --git a/RobotCode/src/main/java/frc/robot/Commands/IntakeCommand.java b/RobotCode/src/main/java/frc/robot/Commands/IntakeCommand.java new file mode 100644 index 00000000..fb166628 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -0,0 +1,59 @@ +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.ClawConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.ArmSubsytem; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ClawSubsystem; + +public class IntakeCommand extends Command { + + private IntakeSubsystem intake; + private ArmSubsytem arm; + private ElevatorSubsystem elevator; + private ClawSubsystem claw; + private boolean limitSwitchPressed; + private double desiredVelocity; + + + public IntakeCommand(IntakeSubsystem intake, double desiredVelocity) { + this.intake = intake; + this.desiredVelocity = desiredVelocity; + + addRequirements(intake); + } + + @Override + public void initialize() { + + intake.setVelocity(IntakeConstants.INTAKING_VELOCITY); + intake.setRotation(IntakeConstants.INTAKE_OUT_ROTATION); + elevator.setTargetPosition(ElevatorConstants.INTAKE_HEIGHT); + claw.setClawSpeed(ClawConstants.CLAW_INTAKE_VELOCITY); + arm.setTargetRotation(ArmConstants.ARM_INTAKE_POSITION); + + + } + + @Override + public void execute() { + limitSwitchPressed = claw.getLimitSwitch(); + if(limitSwitchPressed){ + claw.setClawSpeed(0); + intake.setVelocity(desiredVelocity); + intake.setRotation(IntakeConstants.INTAKE_IN_ROTATION); + } + } + + @Override + public boolean isFinished() { + return limitSwitchPressed; + } + + +} + diff --git a/RobotCode/src/main/java/frc/robot/RobotContainer.java b/RobotCode/src/main/java/frc/robot/RobotContainer.java index 7ca917aa..d4ee7422 100644 --- a/RobotCode/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode/src/main/java/frc/robot/RobotContainer.java @@ -1,41 +1,114 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.lib.core.LogitechControllerButtons; +import frc.robot.constants.ElevatorPosition; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.ArmSubsytem; +import frc.robot.subsystems.ClawSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ -public class RobotContainer { +public class RobotContainer +{ private static RobotContainer instance; private final ShuffleboardTab ShuffleboardTab; + private ArmSubsytem armSubsytem; + private ClawSubsystem clawSubsystem; + private IntakeSubsystem intakeSubsystem; + private ElevatorSubsystem elevatorSubsystem; + + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { + public RobotContainer() + { instance = this; ShuffleboardTab = Shuffleboard.getTab("Tab 1"); // Instantiate subsystems + armSubsytem = new ArmSubsytem(); + clawSubsystem = new ClawSubsystem(); + intakeSubsystem = new IntakeSubsystem(); + elevatorSubsystem = new ElevatorSubsystem(); // Configure the button bindings configurePrimaryBindings(); configureSecondaryBindings(); } - private void configurePrimaryBindings() { + private void configurePrimaryBindings() + { - } + final Joystick controller = new Joystick(0); + + // TODO: REGISTER SUBSYSTEMS and I think make them final (check github) + + final JoystickButton LeftTrigger = new JoystickButton(controller, + LogitechControllerButtons.triggerLeft); + final JoystickButton RightTrigger = new JoystickButton(controller, + LogitechControllerButtons.triggerRight); + final JoystickButton LeftBumper = new JoystickButton(controller, + LogitechControllerButtons.bumperLeft); + final JoystickButton BumperRight = new JoystickButton(controller, + LogitechControllerButtons.bumperRight); + final JoystickButton AButton = new JoystickButton(controller, LogitechControllerButtons.a); + final JoystickButton BButton = new JoystickButton(controller, LogitechControllerButtons.b); + final JoystickButton XButton = new JoystickButton(controller, LogitechControllerButtons.x); + final JoystickButton YButton = new JoystickButton(controller, LogitechControllerButtons.y); + final JoystickButton UpButton = new JoystickButton(controller, + LogitechControllerButtons.up); + final JoystickButton DownButton = new JoystickButton(controller, + LogitechControllerButtons.down); + final JoystickButton LeftButton = new JoystickButton(controller, + LogitechControllerButtons.left); + final JoystickButton RightButton = new JoystickButton(controller, + LogitechControllerButtons.right); + + // Claw buttons + RightTrigger.whileTrue(clawSubsystem.clawCommand(0)); + + BumperRight.whileTrue(clawSubsystem.clawCommand(1.0)); + + // Elevator buttons + YButton.whileTrue(elevatorSubsystem.elevatorCommand(ElevatorPosition.UPPER)); + + XButton.whileTrue(elevatorSubsystem.elevatorCommand(ElevatorPosition.MIDDLE)); + AButton.whileTrue(elevatorSubsystem.elevatorCommand(ElevatorPosition.LOWER)); + + // Intake buttons + LeftTrigger.whileTrue(intakeSubsystem.intakeCommand(IntakeConstants.INTAKING_VELOCITY)); + + BButton.whileTrue(intakeSubsystem.intakeCommand(IntakeConstants.INTAKING_VELOCITY)); + + LeftBumper.whileTrue(intakeSubsystem.intakeCommand(IntakeConstants.INTAKING_VELOCITY)); + + // Arm buttons + DownButton.whileTrue(armSubsytem.armCommand(-1)); + UpButton.whileTrue(armSubsytem.armCommand(1)); + LeftButton.whileTrue(armSubsytem.armCommand(0)); + RightButton.whileTrue(armSubsytem.armCommand(0)); - private void configureSecondaryBindings() { } + private void configureSecondaryBindings() + {} + @SuppressWarnings("unused") - public static ShuffleboardTab getShuffleboardTab() { + public static ShuffleboardTab getShuffleboardTab() + { return instance.ShuffleboardTab; } diff --git a/RobotCode/src/main/java/frc/robot/commands/ArmCommand.java b/RobotCode/src/main/java/frc/robot/commands/ArmCommand.java new file mode 100644 index 00000000..1907671d --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/commands/ArmCommand.java @@ -0,0 +1,25 @@ +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ArmConstants; +import frc.robot.subsystems.ArmSubsytem; + +public class ArmCommand extends Command +{ + public ArmSubsytem arm; + public double targetAngle; + + public ArmCommand(ArmSubsytem arm, double targetAngle) + { + this.targetAngle = targetAngle; + + this.arm = arm; + addRequirements(arm); + + } + + @Override + public void initialize(){ + arm.setTargetRotation(targetAngle); + } +} diff --git a/RobotCode/src/main/java/frc/robot/commands/ClawCommand.java b/RobotCode/src/main/java/frc/robot/commands/ClawCommand.java new file mode 100644 index 00000000..7ff76466 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/commands/ClawCommand.java @@ -0,0 +1,31 @@ +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ClawConstants; +import frc.robot.subsystems.ClawSubsystem; + +public class ClawCommand extends Command +{ + private ClawSubsystem claw; + private double desiredVelocity; + + public ClawCommand(ClawSubsystem claw, double desiredVelocity) + { + this.claw = claw; + this.desiredVelocity = desiredVelocity; + addRequirements(claw); + } + + @Override + public void initialize() + { + claw.setClawSpeed(desiredVelocity); + } + + @Override + public void end(boolean stop) + { + claw.setClawSpeed(ClawConstants.CLAW_REST_VELOCITY); + + } +} diff --git a/RobotCode/src/main/java/frc/robot/commands/ElevateCommand.java b/RobotCode/src/main/java/frc/robot/commands/ElevateCommand.java new file mode 100644 index 00000000..fd60044c --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/commands/ElevateCommand.java @@ -0,0 +1,26 @@ +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ElevatorPosition; +import frc.robot.subsystems.ElevatorSubsystem; + +public class ElevateCommand extends Command { + + private ElevatorSubsystem elevatorSubsystem; + private double targetPosition; + + public ElevateCommand(ElevatorSubsystem subsystem, double position) { + + this.elevatorSubsystem = subsystem; + this.targetPosition = position; + + this.addRequirements( this.elevatorSubsystem ); + + } + + @Override + public void initialize() { + elevatorSubsystem.setTargetPosition( this.targetPosition ); + } + +} diff --git a/RobotCode/src/main/java/frc/robot/constants/ArmConstants.java b/RobotCode/src/main/java/frc/robot/constants/ArmConstants.java new file mode 100644 index 00000000..ee0a5be9 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/constants/ArmConstants.java @@ -0,0 +1,23 @@ +package frc.robot.constants; + +public class ArmConstants +{ + + public static final double ARM_KP = 0; + public static final double ARM_KI = 0; + public static final double ARM_KD = 0; + public static final double ARM_KS = 0; + public static final double ARM_KV = 0; + public static final double ARM_KA = 0; + public static final double ARM_KG = 0; + + public static final double ARM_MIN_ANGLE = 0; + + public static final double ARM_INITIAL_POSITION = 0; + + public static final double ARM_INTAKE_POSITION = 0; + + public static final double ARM_DEADBAND = 0; + public static final double ARM_CURRENT_LIMIT = 0; + +} diff --git a/RobotCode/src/main/java/frc/robot/constants/ClawConstants.java b/RobotCode/src/main/java/frc/robot/constants/ClawConstants.java new file mode 100644 index 00000000..1aa5fa56 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/constants/ClawConstants.java @@ -0,0 +1,17 @@ +package frc.robot.constants; + +public class ClawConstants +{ + + public static final double CLAW_MOTOR_KP = 0; + public static final double CLAW_MOTOR_KI = 0; + public static final double CLAW_MOTOR_KD = 0; + + public static final double CLAW_REST_VELOCITY = 0; + public static final double CLAW_DESIRED_VELOCITY = 0; + + public static final int CLAW_MOTOR_CURRENT_LIMIT = 0; + + public static final double CLAW_INTAKE_VELOCITY = 0; + +} diff --git a/RobotCode/src/main/java/frc/robot/constants/Constants.java b/RobotCode/src/main/java/frc/robot/constants/Constants.java index 4a607fc2..789c9b68 100644 --- a/RobotCode/src/main/java/frc/robot/constants/Constants.java +++ b/RobotCode/src/main/java/frc/robot/constants/Constants.java @@ -1,6 +1,5 @@ package frc.robot.constants; -public class Constants -{ - +public class Constants { + } diff --git a/RobotCode/src/main/java/frc/robot/constants/ElevatorConstants.java b/RobotCode/src/main/java/frc/robot/constants/ElevatorConstants.java new file mode 100644 index 00000000..7272d60c --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -0,0 +1,31 @@ +package frc.robot.constants; + +public class ElevatorConstants { + + public static final double ACCEPTABLE_TARGET_DIFFERENCE = 2; + + public static final double ELEVATOR_MOTOR_KP = 0.0; + public static final double ELEVATOR_MOTOR_KI = 0.0; + public static final double ELEVATOR_MOTOR_KD = 0.0; + + public static final double ELEVATOR_MOTOR_KS = 0.0; + public static final double ELEVATOR_MOTOR_KV = 0.0; + public static final double ELEVATOR_MOTOR_KA = 0.0; + + public static final double ELEVATOR_MOTOR_KG = 0.0; + + public static final double ELEVATOR_ACCELERATION = 0.0; + public static final double ELEVATOR_CRUISE_VELOCITY = 0.0; + + + public static final double MIN_ANGLE = 0.0; + public static final double MIN_ANGLE_RAD = MIN_ANGLE * Math.PI/180; + + public static final double MOTOR_ROTATIONS_IN_RADIANS = 0.0; + + public static final double INTAKE_HEIGHT = 0.0; + + + + +} diff --git a/RobotCode/src/main/java/frc/robot/constants/ElevatorPosition.java b/RobotCode/src/main/java/frc/robot/constants/ElevatorPosition.java new file mode 100644 index 00000000..ac3ed6c5 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/constants/ElevatorPosition.java @@ -0,0 +1,9 @@ +package frc.robot.constants; + +public class ElevatorPosition { + + public static final double UPPER = 25.0; + public static final double MIDDLE = 50.0; + public static final double LOWER = 75.0; + +} diff --git a/RobotCode/src/main/java/frc/robot/constants/IntakeConstants.java b/RobotCode/src/main/java/frc/robot/constants/IntakeConstants.java new file mode 100644 index 00000000..b2bb9e98 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/constants/IntakeConstants.java @@ -0,0 +1,19 @@ +package frc.robot.constants; + +public class IntakeConstants +{ + + public static final double INTAKE_KP = 0; + public static final double INTAKE_KI = 0; + public static final double INTAKE_KD = 0; + public static final double INTAKE_KS = 0; + public static final double INTAKE_KV = 0; + public static final double INTAKE_KA = 0; + public static final double INTAKE_CRUISE_VELOCITY = 0; + public static final double INTAKE_ACCELERATION = 0; + + public static final double INTAKING_VELOCITY = 0; + public static final double REST_VELOCITY = 0; + public static final double INTAKE_IN_ROTATION = 0; + public static final double INTAKE_OUT_ROTATION = 0; +} diff --git a/RobotCode/src/main/java/frc/robot/constants/Ports.java b/RobotCode/src/main/java/frc/robot/constants/Ports.java index cbf79b3f..3d04e022 100644 --- a/RobotCode/src/main/java/frc/robot/constants/Ports.java +++ b/RobotCode/src/main/java/frc/robot/constants/Ports.java @@ -1,6 +1,21 @@ package frc.robot.constants; -public final class Ports -{ +public final class Ports { + + public static final int ELEVATOR_MOTOR_LEFT = 0; + public static final int ELEVATOR_MOTOR_RIGHT = 1; + + // these are for subsystems + public static final int clawMotorLeft = 0; + public static final int clawMotorRight = 0; + + + public static final int INTAKE_LEFT_MOTOR = 0; + public static final int INTAKE_RIGHT_MOTOR = 0; + public static final int INTAKE_FEED = 0; + + public static final int ARM_LEFT_MOTOR = 0; + public static final int ARM_RIGHT_MOTOR = 1; + public static final int ARM_GYRO = 2; } diff --git a/RobotCode/src/main/java/frc/robot/subsystems/ArmSubsytem.java b/RobotCode/src/main/java/frc/robot/subsystems/ArmSubsytem.java new file mode 100644 index 00000000..a1f570d5 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/subsystems/ArmSubsytem.java @@ -0,0 +1,94 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.Ports; + +public class ArmSubsytem extends SubsystemBase { + private TalonFX armMotorRight, armMotorLeft; + + public double targetAngle; + + private MotionMagicDutyCycle motorControlRequest; + + public ArmSubsytem() + { + + armMotorRight = new TalonFX(Ports.ARM_RIGHT_MOTOR); + armMotorLeft = new TalonFX(Ports.ARM_LEFT_MOTOR); + + armMotorRight.setControl(new Follower(armMotorLeft.getDeviceID(), true)); + + + TalonFXConfiguration mainMotorConfigs = new TalonFXConfiguration(); + + mainMotorConfigs.CurrentLimits.StatorCurrentLimitEnable = true; + mainMotorConfigs.CurrentLimits.StatorCurrentLimit = ArmConstants.ARM_CURRENT_LIMIT; + + // PIDs + Slot0Configs pidSlot0Configs = mainMotorConfigs.Slot0; + pidSlot0Configs.kP = ArmConstants.ARM_KP; + pidSlot0Configs.kI = ArmConstants.ARM_KI; + pidSlot0Configs.kD = ArmConstants.ARM_KD; + pidSlot0Configs.kS = ArmConstants.ARM_KS; + pidSlot0Configs.kV = ArmConstants.ARM_KV; + pidSlot0Configs.kA = ArmConstants.ARM_KA; + pidSlot0Configs.kG = ArmConstants.ARM_KG; + + armMotorRight.getConfigurator().apply(mainMotorConfigs); + armMotorLeft.getConfigurator().apply(mainMotorConfigs); + + double targetAngle = ArmConstants.ARM_INITIAL_POSITION; + + + + RobotContainer.getShuffleboardTab().add("Actual Arm Mount Rotation", armMotorRight.getRotorPosition().getValueAsDouble()); + RobotContainer.getShuffleboardTab().add("Target Arm Mount Rotation", targetAngle); + + + + } + + public void periodic(){ + if(armMotorRight.hasResetOccurred()||armMotorLeft.hasResetOccurred()){ + armMotorRight.optimizeBusUtilization(); + armMotorLeft.optimizeBusUtilization(); + + armMotorLeft.getRotorPosition().setUpdateFrequency(20); + } + } + + public void setTargetRotation(double targetAngle) + { + + double gravityFeedForward = Math + .cos(ArmConstants.ARM_MIN_ANGLE + Math.toDegrees(targetAngle)) + * ArmConstants.ARM_KG; + + armMotorLeft.setControl(motorControlRequest.withPosition(targetAngle) + .withFeedForward(gravityFeedForward)); + + } + + public boolean isAtTarget() + { + if (Math.abs(armMotorLeft.getRotorPosition().getValueAsDouble() - (targetAngle)) < ArmConstants.ARM_DEADBAND) + { + return true; + } + return false; + } + public Command armCommand(double targetAngle){ + return Commands.runOnce(() -> setTargetRotation(targetAngle)); + } + +} diff --git a/RobotCode/src/main/java/frc/robot/subsystems/ClawSubsystem.java b/RobotCode/src/main/java/frc/robot/subsystems/ClawSubsystem.java new file mode 100644 index 00000000..179ca3df --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/subsystems/ClawSubsystem.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.FaultID; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; +import frc.robot.constants.ClawConstants; +import frc.robot.constants.Ports; + +public class ClawSubsystem extends SubsystemBase +{ + private SparkPIDController clawPidController; + private CANSparkMax clawMotorLeft, clawMotorRight; + private double desiredSpeed; + private DigitalInput limitSwitch; + public ClawSubsystem() + { + CANSparkMax clawMotorLeft = new CANSparkMax(Ports.clawMotorLeft, MotorType.kBrushless); + CANSparkMax clawMotorRight = new CANSparkMax(Ports.clawMotorRight, MotorType.kBrushless); + clawMotorLeft.follow(clawMotorRight); + clawMotorLeft.setInverted(true); + + clawMotorRight.setSmartCurrentLimit(ClawConstants.CLAW_MOTOR_CURRENT_LIMIT); + clawMotorLeft.setSmartCurrentLimit(ClawConstants.CLAW_MOTOR_CURRENT_LIMIT); + + clawPidController = clawMotorRight.getPIDController(); + clawPidController.setP(ClawConstants.CLAW_MOTOR_KP); + clawPidController.setI(ClawConstants.CLAW_MOTOR_KI); + clawPidController.setD(ClawConstants.CLAW_MOTOR_KD); + + } + + public void periodic() + { + // sticky faults are set to true when a motor has a fault. + if (clawMotorLeft.getStickyFault(FaultID.kHasReset) + || clawMotorRight.getStickyFault(FaultID.kHasReset)) + { + clawMotorLeft.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 20); + clawMotorLeft.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 20); + + clawMotorRight.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 20); + clawMotorRight.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 20); + + clawMotorLeft.setIdleMode(CANSparkMax.IdleMode.kBrake); + clawMotorRight.setIdleMode(CANSparkMax.IdleMode.kBrake); + + RobotContainer.getShuffleboardTab().addDouble("Desired Claw Velocity", + () -> desiredSpeed); + RobotContainer.getShuffleboardTab().addDouble("Actual Claw Motor Velocity", + () -> clawMotorRight.getEncoder().getVelocity()); + } + } + + public void setClawSpeed(double desiredSpeed) + { + this.desiredSpeed = desiredSpeed; + clawPidController.setReference(desiredSpeed, ControlType.kVelocity); + } + + public boolean getLimitSwitch() + { + if (limitSwitch.get()) + { + return true; + } + else + { + return false; + } + } + + public Command clawCommand(double desiredVelocity) + { + return Commands.startEnd(() -> setClawSpeed(desiredVelocity), + () -> setClawSpeed(ClawConstants.CLAW_REST_VELOCITY)); + } + + + + + +} \ No newline at end of file diff --git a/RobotCode/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/RobotCode/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 00000000..8ac18c38 --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,82 @@ +package frc.robot.subsystems; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.ElevatorPosition; +import frc.robot.constants.Ports; + +public class ElevatorSubsystem extends SubsystemBase { + + + private TalonFX leftMotor, rightMotor; + private double elevatorPosition = ElevatorPosition.LOWER; + + private MotionMagicDutyCycle motorControlRequest; + + public ElevatorSubsystem() { + + // Arbitrary values :D + this.leftMotor = new TalonFX(Ports.ELEVATOR_MOTOR_LEFT); + this.rightMotor = new TalonFX(Ports.ELEVATOR_MOTOR_RIGHT); + + this.rightMotor.setControl(new Follower(Ports.ELEVATOR_MOTOR_LEFT, true)); + + TalonFXConfiguration mainMotorConfigs = new TalonFXConfiguration(); + + Slot0Configs pidSlot0Configs = mainMotorConfigs.Slot0; + pidSlot0Configs.kP = ElevatorConstants.ELEVATOR_MOTOR_KP; + pidSlot0Configs.kI = ElevatorConstants.ELEVATOR_MOTOR_KI; + pidSlot0Configs.kD = ElevatorConstants.ELEVATOR_MOTOR_KD; + pidSlot0Configs.kS = ElevatorConstants.ELEVATOR_MOTOR_KS; + pidSlot0Configs.kV = ElevatorConstants.ELEVATOR_MOTOR_KV; + pidSlot0Configs.kA = ElevatorConstants.ELEVATOR_MOTOR_KA; + + MotionMagicConfigs motionMagicVelocityConfigs = mainMotorConfigs.MotionMagic; + motionMagicVelocityConfigs.MotionMagicCruiseVelocity = ElevatorConstants.ELEVATOR_CRUISE_VELOCITY; + motionMagicVelocityConfigs.MotionMagicAcceleration = ElevatorConstants.ELEVATOR_ACCELERATION; + + leftMotor.getConfigurator().apply(mainMotorConfigs); + + motorControlRequest = new MotionMagicDutyCycle(elevatorPosition); + leftMotor.setControl(motorControlRequest.withPosition(elevatorPosition)); + + } + + + + /** + * Sets the elevator's target to a preset position. + * @param position The preset position to set the robot to. + */ + public void setTargetPosition(double position) { + + this.elevatorPosition = position; + leftMotor.setControl(motorControlRequest.withPosition(this.elevatorPosition) ); + + } + + + + /** + * @return Returns where the motor is in encoder ticks. + */ + public double getMotorPosition() { + + return leftMotor.getRotorPosition().getValueAsDouble(); + + } + public Command elevatorCommand(double targetPosition) { + return Commands.runOnce(() -> setTargetPosition(targetPosition)); + } + + + +} diff --git a/RobotCode/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/RobotCode/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 00000000..eb6bde9a --- /dev/null +++ b/RobotCode/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,129 @@ +package frc.robot.subsystems; + + +import java.util.function.BooleanSupplier; + +import javax.swing.plaf.TreeUI; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.FaultID; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.ClawConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.IntakeConstants; +import frc.robot.constants.Ports; + +public class IntakeSubsystem extends SubsystemBase { + // This makes variables for the 3 essential motors + private TalonFX intakeMotorLeft, intakeMotorRight; + private CANSparkMax intakeFeedMotor; + private double desiredRotation, desiredVelocity; + private MotionMagicDutyCycle motorControlRequest; + + private IntakeSubsystem intake; + private ClawSubsystem claw; + private boolean limitSwitchPressed; + + private ArmSubsytem arm; + private ElevatorSubsystem elevator; + + BooleanSupplier limitSwitch; + + + + public IntakeSubsystem() { + // This sets up the 3 essential motors + intakeMotorLeft = new TalonFX(Ports.INTAKE_LEFT_MOTOR); + intakeMotorRight = new TalonFX(Ports.INTAKE_RIGHT_MOTOR); + intakeFeedMotor = new CANSparkMax(Ports.INTAKE_FEED, MotorType.kBrushless); + + desiredRotation = 0; + + intakeMotorLeft.setControl(new Follower(intakeMotorRight.getDeviceID(), true)); + + TalonFXConfiguration motorConfigs = new TalonFXConfiguration(); + Slot0Configs pidSlot0Configs = motorConfigs.Slot0; + pidSlot0Configs.kP = IntakeConstants.INTAKE_KP; + pidSlot0Configs.kI = IntakeConstants.INTAKE_KI; + pidSlot0Configs.kD = IntakeConstants.INTAKE_KD; + pidSlot0Configs.kS = IntakeConstants.INTAKE_KS; + pidSlot0Configs.kV = IntakeConstants.INTAKE_KV; + pidSlot0Configs.kA = IntakeConstants.INTAKE_KA; + + MotionMagicConfigs motionMagicVelocityConfigs = motorConfigs.MotionMagic; + motionMagicVelocityConfigs.MotionMagicCruiseVelocity = IntakeConstants.INTAKE_CRUISE_VELOCITY; + motionMagicVelocityConfigs.MotionMagicAcceleration = IntakeConstants.INTAKE_ACCELERATION; + + motorControlRequest = new MotionMagicDutyCycle(desiredRotation); + intakeMotorRight.setControl(motorControlRequest.withPosition(desiredRotation)); + intakeMotorRight.getConfigurator().apply(motorConfigs); + + + + } + public void periodic(){ + if(intakeMotorLeft.hasResetOccurred()||intakeMotorRight.hasResetOccurred()){ + intakeMotorLeft.optimizeBusUtilization(); + intakeMotorRight.optimizeBusUtilization(); + intakeMotorRight.getRotorPosition().setUpdateFrequency(20); + } + if(intakeFeedMotor.getStickyFault(FaultID.kHasReset)){ + intakeFeedMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 20); + intakeFeedMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 20); + } + + } + public void setRotation(double desiredRotation){ + this.desiredRotation = desiredRotation; + intakeMotorRight.setControl(motorControlRequest.withPosition(desiredRotation)); + } + public void setVelocity(double desiredVelocity) + { + this.desiredVelocity = desiredVelocity; + intakeFeedMotor.set(IntakeConstants.INTAKE_KV + desiredVelocity); + } + public Command initalizeIntake(){ + return Commands.runOnce(() -> { + intake.setVelocity(IntakeConstants.INTAKING_VELOCITY); + intake.setRotation(IntakeConstants.INTAKE_OUT_ROTATION); + elevator.setTargetPosition(ElevatorConstants.INTAKE_HEIGHT); + claw.setClawSpeed(ClawConstants.CLAW_INTAKE_VELOCITY); + arm.setTargetRotation(ArmConstants.ARM_INTAKE_POSITION); + }); + } + + + + public Command intakeCommand(double desiredVelocity) { + return Commands.run(() -> { + limitSwitchPressed = claw.getLimitSwitch(); + if(limitSwitchPressed){ + claw.setClawSpeed(0); + intake.setVelocity(desiredVelocity); + intake.setRotation(IntakeConstants.INTAKE_IN_ROTATION); + } + + }, this).beforeStarting(initalizeIntake()); + } + +} + + + + + + + +