Skip to content

Algae Shooter Pivot and Flywheel Code #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
pix-xl merged 2 commits into from
Feb 1, 2025
Merged

Algae Shooter Pivot and Flywheel Code #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dc64eed
Algae Shooter Pivot and Flywheel Code
joelzoto Jan 28, 2025
f7b96aa
Merge branch 'main' of https://github.com/joelzoto/2025RobotCode
joelzoto Jan 28, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 87 additions & 0 deletions src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}
}



87 changes: 87 additions & 0 deletions src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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());
}
}
35 changes: 35 additions & 0 deletions vendordeps/AdvantageKit.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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": []
}