Skip to content

Tested out Indexer and Flywheel Commands #11

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
Eddie-Edward merged 2 commits into from
Feb 27, 2025
Merged

Tested out Indexer and Flywheel Commands #11

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
d0304c2
Tested out Flywheel and Indexer subsystems
joelzoto Feb 27, 2025
efe1433
Merge branch 'main' of https://github.com/joelzoto/2025RobotCode
joelzoto Feb 27, 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
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.crevolib.configs.CTREConfigs;
import frc.robot.driver.Driver;
import frc.robot.driver.DriverXbox;
import frc.robot.operator.OperatorXbox;

Expand Down Expand Up @@ -46,7 +47,7 @@ public static void resetCommandsAndButtons() {
// Reset Config for all gamepads and other button bindings
// Driver.getInstance().resetConfig();
// Operator.getInstance().resetConfig();
DriverXbox.getInstance().resetConfig();
Driver.getInstance().resetConfig();
OperatorXbox.getInstance().resetConfig();
}

Expand Down
24 changes: 12 additions & 12 deletions src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,11 @@
/** 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 int kLeftId = 15;
static final int kRightId = 16;

static final InvertedValue kAlgaeScoringInverted = InvertedValue.Clockwise_Positive;
static final InvertedValue kAlgaeIntakingInverted = InvertedValue.CounterClockwise_Positive;
public static final InvertedValue kAlgaeScoringInverted = InvertedValue.Clockwise_Positive;
public static final InvertedValue kAlgaeIntakingInverted = InvertedValue.CounterClockwise_Positive;

static final Slot0Configs kAlgaeFlyWheelConfigs = new Slot0Configs()
.withKS(0.0)
Expand All @@ -44,15 +44,15 @@ public static class Settings {


private AlgaeFlyWheel() {
LeftFlyWheel = new TalonFX(Settings.kLeftId, "Canivore");
var leftTalonFXConfigurator = LeftFlyWheel.getConfigurator();
LeftFlyWheel = new TalonFX(Settings.kLeftId);
leftTalonFXConfigurator = LeftFlyWheel.getConfigurator();
leftMotorConfigs = new MotorOutputConfigs();

leftTalonFXConfigurator.apply(Settings.kAlgaeFlyWheelConfigs);


RightFlyWheel = new TalonFX(Settings.kRightId, "Canivore");
var rightTalonFXConfigurator = RightFlyWheel.getConfigurator();
RightFlyWheel = new TalonFX(Settings.kRightId);
rightTalonFXConfigurator = RightFlyWheel.getConfigurator();
rightMotorConfigs = new MotorOutputConfigs();

rightTalonFXConfigurator.apply(Settings.kAlgaeFlyWheelConfigs);
Expand All @@ -65,17 +65,17 @@ public static AlgaeFlyWheel getInstance() {
return mInstance;
}

public void setLeftFlywheelVelocity(Rotation2d velocity, InvertedValue kInvertedValue) {
public void setLeftFlywheelVelocity(Rotation2d velocity, InvertedValue kLeftInvertedValue) {
// set invert to CW+ and apply config change
leftMotorConfigs.Inverted = kInvertedValue;
leftMotorConfigs.Inverted = kLeftInvertedValue;
leftTalonFXConfigurator.apply(leftMotorConfigs);

LeftFlyWheel.setControl(new VelocityVoltage(velocity.getRotations()));
}

public void setRightFlywheelVelocity(Rotation2d velocity, InvertedValue kInvertedValue) {
public void setRightFlywheelVelocity(Rotation2d velocity, InvertedValue kRightInvertedValue) {
// set invert to CW+ and apply config change
rightMotorConfigs.Inverted = kInvertedValue;
rightMotorConfigs.Inverted = kRightInvertedValue;
rightTalonFXConfigurator.apply(rightMotorConfigs);

RightFlyWheel.setControl(new VelocityVoltage(velocity.getRotations()));
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,11 @@
import com.ctre.phoenix6.signals.InvertedValue;

public class AlgaeFlyWheelCommands {
public static Command setAngularVelocity(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kInvertedValue) {
return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kInvertedValue);
public static Command setAngularVelocity(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kLeftInvertedValue, kRightInvertedValue);
}

public static Command setAngularVelocity(Supplier<Rotation2d> velocitySupplier, InvertedValue kInvertedValue) {
return setAngularVelocity(velocitySupplier, velocitySupplier, kInvertedValue);
public static Command setAngularVelocity(Supplier<Rotation2d> velocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
return setAngularVelocity(velocitySupplier, velocitySupplier, kLeftInvertedValue, kRightInvertedValue);
}
}
19 changes: 10 additions & 9 deletions src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,27 +14,28 @@
public class SetVelocityAlgaeFlyWheel extends Command {
private final AlgaeFlyWheel flywheel;
private final Supplier<Rotation2d> leftVelocitySupplier, rightVelocitySupplier;
private final InvertedValue kInvertedValue;
private final InvertedValue kLeftInvertedValue, kRightInvertedValue;

private final Rotation2d kAllowedError = Rotation2d.fromRotations(5); // 300 RPM

SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kInvertedValue) {
SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
flywheel = AlgaeFlyWheel.getInstance();
this.leftVelocitySupplier = leftVelocitySupplier;
this.rightVelocitySupplier = rightVelocitySupplier;
this.kInvertedValue = kInvertedValue;
this.kLeftInvertedValue = kLeftInvertedValue;
this.kRightInvertedValue = kRightInvertedValue;
}

SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> velocitySupplier, InvertedValue kInvertedValue) {
this(velocitySupplier, velocitySupplier, kInvertedValue);
SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> velocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
this(velocitySupplier, velocitySupplier, kLeftInvertedValue, kRightInvertedValue);
}

@Override
public void execute() {
final var leftVel = leftVelocitySupplier.get();
final var rightVel = rightVelocitySupplier.get();
flywheel.setRightFlywheelVelocity(leftVel, kInvertedValue);
flywheel.setLeftFlywheelVelocity(rightVel, kInvertedValue);
flywheel.setRightFlywheelVelocity(leftVel, kRightInvertedValue);
flywheel.setLeftFlywheelVelocity(rightVel, kLeftInvertedValue);

SmartDashboard.putBoolean("Shooter Ready (left)", (Math.abs(leftVel.getRotations()) - (Math.abs(flywheel.getLeftFlywheelVelocity().getRotations()))) < kAllowedError.getRotations());
SmartDashboard.putBoolean("Shooter Ready (right)", (Math.abs(rightVel.getRotations()) - (Math.abs(flywheel.getRightFlywheelVelocity().getRotations()))) < kAllowedError.getRotations());
Expand All @@ -55,7 +56,7 @@ public boolean isFinished() {

@Override
public void end(boolean isInterrupted) {
flywheel.setRightFlywheelVelocity(Rotation2d.fromDegrees(0), kInvertedValue);
flywheel.setLeftFlywheelVelocity(Rotation2d.fromDegrees(0), kInvertedValue);
flywheel.setRightFlywheelVelocity(Rotation2d.fromDegrees(0), kRightInvertedValue);
flywheel.setLeftFlywheelVelocity(Rotation2d.fromDegrees(0), kLeftInvertedValue);
}
}
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
Expand Down Expand Up @@ -48,12 +49,14 @@ public class Settings{
private Constraints mConstraints;
private final ProfiledPIDController mPPIDController;
private final ArmFeedforward mFFController;
private MotorOutputConfigs motorConfigs;
private TalonFXConfigurator talonFXConfigurator;

public AlgaeShooterPivot() {
mKraken = new TalonFX(Settings.kAlgaePivotTalonID,"Canivore");

var talonFXConfigurator = mKraken.getConfigurator();
var motorConfigs = new MotorOutputConfigs();
talonFXConfigurator = mKraken.getConfigurator();
motorConfigs = new MotorOutputConfigs();

motorConfigs.Inverted = InvertedValue.Clockwise_Positive;
talonFXConfigurator.apply(motorConfigs);
Expand Down
15 changes: 14 additions & 1 deletion src/main/java/frc/robot/driver/Driver.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,13 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.crevolib.util.ExpCurve;
import frc.crevolib.util.Gamepad;
import frc.crevolib.util.XboxGamepad;
import frc.robot.Robot;
import frc.robot.algaeflywheel.AlgaeFlyWheel;
import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands;
import frc.robot.indexer.commands.IndexerCommands;

public class Driver extends Gamepad {
public class Driver extends XboxGamepad {
private static class Settings {
static final int port = 0;
static final String name = "driver";
Expand Down Expand Up @@ -38,9 +42,18 @@ public static Driver getInstance() {

@Override
public void setupTeleopButtons() {
controller.a().whileTrue(
AlgaeFlyWheelCommands.setAngularVelocity(
() -> AlgaeFlyWheel.Settings.kMaxAngularVelocity.times(0.8),
AlgaeFlyWheel.Settings.kAlgaeScoringInverted,
AlgaeFlyWheel.Settings.kAlgaeIntakingInverted
));

controller.b().whileTrue(IndexerCommands.setOutput(() ->0.10));
// Drivetrain Commands



}

@Override
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/driver/DriverXbox.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@
import frc.crevolib.util.ExpCurve;
import frc.crevolib.util.XboxGamepad;
import frc.robot.Robot;
import frc.robot.algaeflywheel.AlgaeFlyWheel;
import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands;
import frc.robot.algaeflywheel.commands.SetVelocityAlgaeFlyWheel;


public class DriverXbox extends XboxGamepad {
Expand All @@ -31,6 +34,7 @@ private static class Settings {
private static ExpCurve rotationStickCurve;
public boolean autoAim;
private double reqAngularVel;


private DriverXbox() {
super(DriverXbox.Settings.name, DriverXbox.Settings.port);
Expand All @@ -48,6 +52,7 @@ public static DriverXbox getInstance() {

@Override
public void setupTeleopButtons() {

// Drivetrain Commands


Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/indexer/Indexer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@

public class Indexer extends SubsystemBase{
public static class Settings {
static int kTalonID = 10;
static int kTalonID = 17;
static final double kMaxVoltage = 12.0;
static final int kCurrentLimit = 200;
static final int kCurrentThreshold = 100;
Expand All @@ -25,7 +25,7 @@ public Indexer() {
var talonFXConfigurator = mKraken.getConfigurator();
var motorConfigs = new MotorOutputConfigs();

motorConfigs.Inverted = InvertedValue.Clockwise_Positive;
motorConfigs.Inverted = InvertedValue.CounterClockwise_Positive;
talonFXConfigurator.apply(motorConfigs);

}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/vision/Vision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,4 +134,4 @@ private void updateEstimationStdDevs(Optional<EstimatedRobotPose> estimatedPose,
public Matrix<N3, N1> getEstimationStdDevs() {
return curStdDevs;
}
}
}