Skip to content

Fixed Algae Shooter Pivot and wrote the set angle command. #4

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
joelzoto merged 2 commits into from
Feb 1, 2025
Merged

Fixed Algae Shooter Pivot and wrote the set angle command. #4

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
34ff9c1
Fixed Algae Shooter Pivot and wrote the set angle command.
joelzoto Feb 1, 2025
bb97ecc
Merge branch 'main' of https://github.com/joelzoto/2025RobotCode
joelzoto Feb 1, 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
120 changes: 84 additions & 36 deletions src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,44 +6,51 @@

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.CANcoder;
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.wpilibj.smartdashboard.SmartDashboard;
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 int kAlgaePivotTalonID = 19;
public static int kAlgaePivotEncoder = 1;
public static boolean isInverted = false;

static final Slot0Configs kAlgaeShooterPivotConfigs = new Slot0Configs()
.withKG(0.0)
.withKS(0.0)
.withKV(0.115)
.withKV(0.0)
.withKA(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.0);

public static final Rotation2d kMaxAngularVelocity = Rotation2d.fromDegrees(200); //120
public static final Rotation2d kMaxAngularAcceleration = Rotation2d.fromDegrees(300);
public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(180);
public static final Rotation2d kMaxAnglePhysical = Rotation2d.fromDegrees(175);

public static final int KMaxVoltage = 30;

// kFFAngleOffset is the differnce between our zero point (handoff) and 'true' zero (parallel to the ground)
private static final Rotation2d kFFAngleOffset = Rotation2d.fromDegrees(30);
}
private static AlgaeShooterPivot mInstance;
private final CANcoder mEncoder;
private TalonFX mKraken;
private Constraints mConstraints;
private final ProfiledPIDController mPPIDController;
private final ArmFeedforward mFFController;

public AlgaeShooterPivot() {
mKraken = new TalonFX(Settings.kTalonID);
mKraken = new TalonFX(Settings.kAlgaePivotTalonID);

var talonFXConfigurator = mKraken.getConfigurator();
var motorConfigs = new MotorOutputConfigs();
Expand All @@ -52,36 +59,77 @@ public AlgaeShooterPivot() {
talonFXConfigurator.apply(motorConfigs);

mKraken.getConfigurator().apply(Settings.kAlgaeShooterPivotConfigs);

mEncoder = new CANcoder(Settings.kAlgaePivotEncoder, "Canivore");
mConstraints = new Constraints( Settings.kMaxAngularVelocity.getRadians(), Settings.kMaxAngularAcceleration.getRadians());
mPPIDController = new ProfiledPIDController(Settings.kAlgaeShooterPivotConfigs.kP, Settings.kAlgaeShooterPivotConfigs.kI, Settings.kAlgaeShooterPivotConfigs.kD, mConstraints);

mFFController = new ArmFeedforward(Settings.kAlgaeShooterPivotConfigs.kS, Settings.kAlgaeShooterPivotConfigs.kG, Settings.kAlgaeShooterPivotConfigs.kV, Settings.kAlgaeShooterPivotConfigs.kA);

//mEncoder = mKraken.getAbsoluteEncoder();

// 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);
// final TrapezoidProfile m_profile = new TrapezoidProfile(
// new TrapezoidProfile.Constraints(Settings.kMaxAngularVelocity, Settings.kMaxAngularAcceleration)
// );

// 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 void setTargetAngle(Rotation2d angle) {
mPPIDController.setGoal(angle.getRadians());
}
public Rotation2d getPivotVelocity() {
return Rotation2d.fromRotations(mKraken.getVelocity().getValueAsDouble());

public Rotation2d getAngularVelocity() {
return Rotation2d.fromRotations(mEncoder.getVelocity().getValueAsDouble()).div(140.0 * 60.0);
}


public Rotation2d getAngle() {
var pos = mEncoder.getPosition().getValueAsDouble();

// Accounts for encoder gear ratio
pos /= 2.0;
if (pos > Settings.kMaxAnglePhysical.getRotations()) {
pos = 0;
}
return Rotation2d.fromRotations(pos);
}

public void resetAngle() {
mPPIDController.setGoal(getAngle().getRadians());
}

@Override
public void periodic() {
SmartDashboard.putNumber("Shooter Pivot Angle (radians)", getAngle().getRadians());
SmartDashboard.putNumber("Shooter Pivot Angular Velocity (radians / sec)", getAngularVelocity().getRadians());
SmartDashboard.putNumber("Shooter Pivot Angle (degrees)", getAngle().getDegrees());
SmartDashboard.putNumber("Shooter Pivot Angular Velocity (degrees / sec)", getAngularVelocity().getDegrees());
SmartDashboard.putNumber("Profilled PID Controller Vel", mPPIDController.getSetpoint().velocity);

double speed = mPPIDController.calculate(getAngle().getRadians());
speed += mFFController.calculate(getAngle().getRadians() - Settings.kFFAngleOffset.getRadians(), mPPIDController.getSetpoint().velocity);

SmartDashboard.putNumber("mPPIDC + mFFC Output", speed);

mKraken.setVoltage(speed);
}
}
84 changes: 84 additions & 0 deletions src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
// 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.commands;

import java.util.function.Supplier;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.algaeshooterpivot.AlgaeShooterPivot;

/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class SetAngleAlgaePivot extends Command {
AlgaeShooterPivot mAlgaeShooterPivot;
Supplier<Rotation2d> targetSupplier;
boolean singleShot = true;

public enum Preset {
kZero(Rotation2d.fromDegrees(2)),
kHandoff(Rotation2d.fromDegrees(0)),
kHandoffClear(Rotation2d.fromDegrees(5)),
kShooterNear(Rotation2d.fromDegrees(0)),
kShooterMid(Rotation2d.fromDegrees(22.25)),
kShooterFarAuton(Rotation2d.fromDegrees(26)),
kShooterFar(Rotation2d.fromDegrees(25.5)),
kTrap(Rotation2d.fromDegrees(153.15)),
kClimb(Rotation2d.fromDegrees(170)),
kPass(Rotation2d.fromDegrees(10)),
kAmp(Rotation2d.fromDegrees(85));

Rotation2d target;

Preset(Rotation2d target) {
this.target = target;
}

public double getDegrees() {
return target.getDegrees();
}

public Rotation2d getRotation2d() {
return target;
}
}

public SetAngleAlgaePivot(Rotation2d angleIn) {
mAlgaeShooterPivot = AlgaeShooterPivot.getInstance();
targetSupplier = ()-> angleIn;
}
public SetAngleAlgaePivot(Supplier<Rotation2d> angleIn) {
mAlgaeShooterPivot = AlgaeShooterPivot.getInstance();
targetSupplier = angleIn;
}

public SetAngleAlgaePivot(Supplier<Rotation2d> angleIn, boolean singleSet) {
mAlgaeShooterPivot = AlgaeShooterPivot.getInstance();
targetSupplier = angleIn;
singleShot = singleSet;
}

@Override
public void initialize() {


}

@Override
public void execute() {
mAlgaeShooterPivot.setTargetAngle(targetSupplier.get());
}


@Override
public boolean isFinished() {
return singleShot;
}

@Override
public void end(boolean interrupted) {

}
}