diff --git a/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java new file mode 100644 index 0000000..096845c --- /dev/null +++ b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java @@ -0,0 +1,87 @@ +// 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.algaeflywheel; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Add your docs here. */ +public class AlgaeFlyWheel extends SubsystemBase{ + public static class Settings { + static final int kLeftId = 27; + static final int kRightId = 28; + + static final Slot0Configs kFlywheelConfigs = new Slot0Configs() + .withKS(0.0) + .withKV(0.115) + .withKP(0.0); + + // 5800 RPM at motor; 11600 RPM at wheels + public static final Rotation2d kMaxAngularVelocity = Rotation2d.fromRotations(6000.0 / 60.0); + + public static final double kCurrentLimit = 60.0; + } + + private static AlgaeFlyWheel mInstance; + private final TalonFX mKrakenLeft, mKrakenRight; + + private AlgaeFlyWheel() { + mKrakenLeft = new TalonFX(Settings.kLeftId); + var leftTalonFXConfigurator = mKrakenLeft.getConfigurator(); + var leftMotorConfigs = new MotorOutputConfigs(); + + leftMotorConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftTalonFXConfigurator.apply(leftMotorConfigs); + + + mKrakenRight = new TalonFX(Settings.kRightId); + var rightTalonFXConfigurator = mKrakenRight.getConfigurator(); + var righttMotorConfigs = new MotorOutputConfigs(); + + righttMotorConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + rightTalonFXConfigurator.apply(righttMotorConfigs); + } + + // 57 degreedf + + public static AlgaeFlyWheel getInstance() { + if (mInstance == null) { + mInstance = new AlgaeFlyWheel(); + } + return mInstance; + } + + public void setLeftFlywheelVelocity(Rotation2d velocity) { + mKrakenLeft.setControl(new VelocityVoltage(velocity.getRotations())); + } + + public void setRightFlywheelVelocity(Rotation2d velocity) { + mKrakenRight.setControl(new VelocityVoltage(velocity.getRotations())); + } + + public Rotation2d getLeftFlywheelVelocity() { + return Rotation2d.fromRotations(mKrakenLeft.getVelocity().getValueAsDouble()); + } + + public Rotation2d getRightFlywheelVelocity() { + return Rotation2d.fromRotations(mKrakenRight.getVelocity().getValueAsDouble()); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Left Flywheel Velocity (RPM)", mKrakenLeft.getVelocity().getValueAsDouble() * 60); + SmartDashboard.putNumber("Right Flywheel Velocity (RPM)", mKrakenRight.getVelocity().getValueAsDouble() * 60); + } +} + + + diff --git a/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java new file mode 100644 index 0000000..bca0e15 --- /dev/null +++ b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java @@ -0,0 +1,87 @@ +// 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.algaeshooterpivot; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Add your docs here. */ +public class AlgaeShooterPivot extends SubsystemBase{ + public class Settings{ + public static int kTalonID = 19; + public static boolean isInverted = false; + + static final Slot0Configs kAlgaeShooterPivotConfigs = new Slot0Configs() + .withKS(0.0) + .withKV(0.115) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); + } + private static AlgaeShooterPivot mInstance; + private TalonFX mKraken; + + public AlgaeShooterPivot() { + mKraken = new TalonFX(Settings.kTalonID); + + var talonFXConfigurator = mKraken.getConfigurator(); + var motorConfigs = new MotorOutputConfigs(); + + motorConfigs.Inverted = InvertedValue.Clockwise_Positive; + talonFXConfigurator.apply(motorConfigs); + + mKraken.getConfigurator().apply(Settings.kAlgaeShooterPivotConfigs); + + // Trapezoid profile with max velocity 80 rps, max accel 160 rps/s + final TrapezoidProfile m_profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(80, 160) + ); + // Final target of 200 rot, 0 rps + TrapezoidProfile.State m_goal = new TrapezoidProfile.State(200, 0); + TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + + // create a position closed-loop request, voltage output, slot 0 configs + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + + // calculate the next profile setpoint + m_setpoint = m_profile.calculate(0.020, m_setpoint, m_goal); + + // send the request to the device + m_request.Position = m_setpoint.position; + m_request.Velocity = m_setpoint.velocity; + mKraken.setControl(m_request); + } + public static AlgaeShooterPivot getInstance() { + if (mInstance == null) { + mInstance = new AlgaeShooterPivot(); + } + return mInstance; + } + public void setPivotVelocity(Rotation2d velocity) { + mKraken.setControl(new VelocityVoltage(velocity.getRotations())); + } + public Rotation2d getPivotVelocity() { + return Rotation2d.fromRotations(mKraken.getVelocity().getValueAsDouble()); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..03df051 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file