diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17c17cd..5ab5e1c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,8 +19,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.DeferredCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.DeviceIDs.controllerIDs; import frc.robot.commands.AddVisionMeasurement; +import frc.robot.commands.states.Intaking; +import frc.robot.commands.states.RetractIntake; import frc.robot.constants.ConstSystem.constControllers; import frc.robot.subsystems.DriverStateMachine; import frc.robot.subsystems.DriverStateMachine.DriverState; @@ -55,7 +58,8 @@ public class RobotContainer { private final RobotPoses loggedRobotPose = robotPose; public static final Vision subVision = new Vision(); private final Vision loggedSubVision = subVision; - + public static final Intaking intakingInstance = new Intaking(); + public static final RetractIntake RetractingInstance = new RetractIntake(); Command TRY_NONE = Commands.deferredProxy( () -> subStateMachine.tryState(RobotState.NONE)); @@ -95,7 +99,10 @@ private void configDriverBindings() { // subDrivetrain.resetModulesToAbsolute())); conDriver.btn_Back .onTrue(Commands.runOnce(() -> subDrivetrain.resetPose(new Pose2d(0, 0, new Rotation2d())))); - + conDriver.btn_RightTrigger + .whileTrue(intakingInstance); + conDriver.btn_LeftTrigger + .whileTrue(RetractingInstance); // Example Pose Drive conDriver.btn_X .whileTrue(EXAMPLE_POSE_DRIVE) diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java new file mode 100644 index 0000000..b1039a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -0,0 +1,42 @@ +// 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.commands.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; +import frc.robot.constants.ConstRotors; + +public class Intaking extends Command { + /** Creates a new Intaking. */ + public Intaking() { + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.INTAKE_ROLLERS_SPEED); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.STOP); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/states/RetractIntake.java b/src/main/java/frc/robot/commands/states/RetractIntake.java new file mode 100644 index 0000000..13e9907 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/RetractIntake.java @@ -0,0 +1,38 @@ +// 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.commands.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; + +public class RetractIntake extends Command { + /** Creates a new RetractIntake. */ + public RetractIntake() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_RETRACT); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index f16135c..9b15163 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -5,15 +5,56 @@ package frc.robot.constants; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle; /** Add your docs here. */ public class ConstMotion { - public static final double STOP = 0; + // public static final double STOP = 0;/ public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); + public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(125); + public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(0); public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14); + + static { + + INTAKE_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake; + INTAKE_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = INTAKE_PIVOT_DEPLOY.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = INTAKE_PIVOT_RETRACT.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + INTAKE_PIVOT_CONFIGURATION.Slot0.kS = 0.3; + INTAKE_PIVOT_CONFIGURATION.Slot0.kP = 40; + INTAKE_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + INTAKE_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((16.0 / 42.0) * (18.0 / 60.0) * (12.0 / 60.0)); + + HOOD_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + HOOD_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((10.0 / 42.0) * (10.0 / 210.0)); + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(46).in(Rotations); + // Do not change, it's not at zero because the hood is not perfectly at 0 when + // the encoder reads 0 + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(1.6).in(Rotations); + HOOD_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + HOOD_PIVOT_CONFIGURATION.Slot0.kS = 0.2; + HOOD_PIVOT_CONFIGURATION.Slot0.kP = 300; + HOOD_PIVOT_CONFIGURATION.Slot0.kG = 0.1; + HOOD_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + } + } diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 32f527b..90246be 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -7,7 +7,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.fasterxml.jackson.core.filter.TokenFilter.Inclusion; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; @@ -22,6 +23,7 @@ public class ConstRotors { public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration(); + public static final AngularVelocity INTAKE_ROLLERS_SPEED = Units.RPM.of(3); public static final AngularVelocity FLYWHEEL_SHOOTING_SPEED = Units.RPM.of(1000); public static final AngularVelocity INTAKE_ROLLER_SPEED = Units.RPM.of(2000); public static final AngularVelocity INTAKE_TRANSFER_SPEED = Units.RPM.of(3000); @@ -34,22 +36,34 @@ public class ConstRotors { // NeutralModeValue.Coast;/ FLYWHEEL_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kS = 0.14; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kV = 0.11167; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kA = 0; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kP = 0.5; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70; FLYWHEEL_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; FLYWHEEL_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kS = 0.14; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kV = 0.11167; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kA = 0; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kP = 0.5; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70; SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; @@ -59,17 +73,20 @@ public class ConstRotors { INTAKE_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kP = 0.7; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kS = 0.15; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kV = 0.12; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kA = 0; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; } - public static final double STOP = 0; + public static final AngularVelocity STOP = Units.RPM.of(0); } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 8bc1e45..067aafd 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -37,6 +38,8 @@ public class Rotors extends SubsystemBase { final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0); final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0); private boolean flyWheelAtSpeed = false; + + final MotionMagicVelocityVoltage intakeRollersVelocityRequest = new MotionMagicVelocityVoltage(0); // private boolean intakeRollersAtSpeed = false;/ public Rotors() { @@ -77,8 +80,8 @@ public void setSerializerRollersSpeed(AngularVelocity speed) { serializerRollers.setControl(serializerVelocityRequest.withVelocity(speed)); } - public void setIntakeRollersSpeeds(double speed) { - intakeRollersEast.set(speed); + public void setIntakeRollersSpeeds(AngularVelocity speed) { + intakeRollersEast.setControl(intakeRollersVelocityRequest.withVelocity(speed)); intakeRollersWest.setControl(intakeRollerEastFollower); }