Skip to content

Alage pviot code #12

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 28, 2025
Merged

Alage pviot code #12

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
57c5dc3
feat: algae subsystem
jmdeal Feb 28, 2025
408402b
Algae shooter pivot
joelzoto Feb 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
92 changes: 92 additions & 0 deletions simgui-ds.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
]
}
8 changes: 0 additions & 8 deletions src/main/java/frc/robot/Indexer.java
View file
Open in desktop

This file was deleted.

70 changes: 18 additions & 52 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,29 +2,15 @@

import com.ctre.phoenix6.swerve.SwerveRequest;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.driver.DriverXbox;
import frc.robot.drivetrain.CommandSwerveDrivetrain;
import frc.robot.drivetrain.TunerConstants;
// import frc.robot.drivetrain2.Drivetrain;
// import frc.robot.drivetrain2.DrivetrainConfig;
// import frc.robot.drivetrain2.DrivetrainConfig.DriveConstants;
// import frc.robot.drivetrain2.commands.DrivetrainCommands;
import frc.robot.drivetrain.commands.DrivetrainCommands;
import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import frc.robot.subsystems.AlgaeSubsystem;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -33,29 +19,21 @@
* subsystems, commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity

private final double kMaxVelocity = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private final double kMaxAngularVelocity = RotationsPerSecond.of(0.75).in(RadiansPerSecond);

/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
//AutonMaster mAutonMaster = new AutonMaster();

// Gamepads

.withDeadband(kMaxVelocity * 0.1)
.withRotationalDeadband(kMaxAngularVelocity * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors

/* Subsystems */

/* Auton Chooser */
public static SendableChooser<Command> mAutonChooser;

private final AlgaeSubsystem algaeSubsystem = AlgaeSubsystem.getInstance();

public RobotContainer() {
//mAutonChooser = mAutonMaster.getAutonSelector();
setDefaultCommands();


}

/**
Expand All @@ -69,28 +47,16 @@ public Command getAutonomousCommand() {

public void setDefaultCommands() {
final var driver = DriverXbox.getInstance();
// final var driver = DriverXbox.getInstance();

// CommandSwerveDrivetrain.getInstance().setDefaultCommand(
// DrivetrainCommands.drive(
// driver.getDriveTranslation().getX() * MaxSpeed,
// driver.getDriveTranslation().getY() * MaxSpeed,
// driver.getDriveRotation() * MaxAngularRate
// )
// );

CommandSwerveDrivetrain.getInstance().setDefaultCommand(
CommandSwerveDrivetrain.getInstance().applyRequest(() ->
drive.withVelocityX(driver.getDriveTranslation().getX() * MaxSpeed) // Drive forward with negative Y (forward)
.withVelocityY(driver.getDriveTranslation().getY() * MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(driver.getDriveRotation() * MaxAngularRate) // Drive counterclockwise with negative X (left)
)
);

// Drivetrain.getInstance().setDefaultCommand(DrivetrainCommands.drive(
// driver::getDriveTranslation,
// driver::getDriveRotation
// ));
// CommandSwerveDrivetrain.getInstance().setDefaultCommand(
// CommandSwerveDrivetrain.getInstance().applyRequest(() ->
// drive.withVelocityX(driver.getDriveTranslation().getX() * kMaxVelocity) // Drive forward with negative Y (forward)
// .withVelocityY(driver.getDriveTranslation().getY() * kMaxVelocity) // Drive left with negative X (left)
// .withRotationalRate(driver.getDriveRotation() * kMaxAngularVelocity) // Drive counterclockwise with negative X (left)
// )
// );

//algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> (driver.getRightX() + 1) / 2.0));
algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> 1.0));
}
}

20 changes: 7 additions & 13 deletions src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,21 +4,15 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.AlgaeSubsystem;

public class AlgaePivotCommands {
public static Command setAlgaePivotAngle(Supplier<Rotation2d> targetSupplier) {
return new SetAngleAlgaePivot(targetSupplier);
public static Command setAlgaePivotAngle(AlgaeSubsystem.State state) {
return new SetAngleAlgaePivot(state);
}

public static Command setAlgaePivotAngle(SetAngleAlgaePivot.Preset state) {
return new SetAngleAlgaePivot(state.getRotation2d());
}

public static Command setAlgaePivotAngle(Rotation2d angle) {
return new SetAngleAlgaePivot(angle);
}

public static Command setAlgaePivotAngle(Supplier<Rotation2d> velocitySupplier, boolean openLoop) {
return new SetAngleAlgaePivot(velocitySupplier, openLoop);
}
// public static Command setAlgaePivotAngle(Supplier<Rotation2d> velocitySupplier, boolean openLoop) {
// return new SetAngleAlgaePivot(velocitySupplier, openLoop);
// }

}
75 changes: 25 additions & 50 deletions src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,68 +7,43 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.algaepivot.AlgaeShooterPivot;
import frc.robot.subsystems.AlgaeSubsystem;

public class SetAngleAlgaePivot extends Command {
AlgaeShooterPivot mAlgaeShooterPivot;
Supplier<Rotation2d> targetSupplier;
boolean singleShot = true;
AlgaeSubsystem mAlgaeSubsystem;
AlgaeSubsystem.State targetState;

public enum Preset {
kZero(Rotation2d.fromDegrees(2)),
kStowed(Rotation2d.fromDegrees(0)),
kAlgaeL2(Rotation2d.fromDegrees(0)),
kAlgaeL3(Rotation2d.fromDegrees(0)),
kCoralL1(Rotation2d.fromDegrees(0)),
kCoralL2(Rotation2d.fromDegrees(5)),
kCoralL3(Rotation2d.fromDegrees(0)),
kCoralL4(Rotation2d.fromDegrees(22.25)),
kAlgaeShoot(Rotation2d.fromDegrees(0));
// public enum State {
// kFloorIntake(Rotation2d.fromRotations(-0.04)),
// kReefIntake(Rotation2d.fromRotations(0)),
// kScore(Rotation2d.fromRotations(0.15)),
// kStow(Rotation2d.fromRotations(0.23));

Rotation2d target;
// State(Rotation2d pos) {
// this.pos = pos;
// }
// public final Rotation2d pos;


Preset(Rotation2d target) {
this.target = target;
}
// public double getDegrees() {
// return pos.getDegrees();
// }

public double getDegrees() {
return target.getDegrees();
}
// public Rotation2d getRotation2d() {
// return pos;
// }
// }

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;
public SetAngleAlgaePivot(AlgaeSubsystem.State targetState) {
mAlgaeSubsystem = AlgaeSubsystem.getInstance();
this.targetState = targetState;
addRequirements(mAlgaeSubsystem);
}

@Override
public void initialize() {


}

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

mAlgaeSubsystem.setTargetPosition(targetState.pos);

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

@Override
Expand Down
17 changes: 16 additions & 1 deletion src/main/java/frc/robot/driver/DriverXbox.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.driver;

import java.util.Optional;
import java.util.Set;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
Expand All @@ -17,8 +18,11 @@
import frc.robot.algaeflywheel.AlgaeFlyWheel;
import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands;
import frc.robot.algaeflywheel.commands.SetVelocityAlgaeFlyWheel;
import frc.robot.algaepivot.commands.AlgaePivotCommands;
import frc.robot.algaepivot.commands.SetAngleAlgaePivot;
import frc.robot.drivetrain2.Drivetrain;
import frc.robot.indexer.commands.IndexerCommands;
import frc.robot.subsystems.AlgaeSubsystem;


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


private DriverXbox() {
Expand All @@ -51,6 +56,7 @@ public static DriverXbox getInstance() {
}
return mInstance;
}


@Override
public void setupTeleopButtons() {
Expand All @@ -64,6 +70,13 @@ public void setupTeleopButtons() {
// controller.b().whileTrue(IndexerCommands.setOutput(() ->0.10));

// controller.y().onTrue(new InstantCommand(() -> Drivetrain.getInstance().zeroHeading()));


controller.a().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kFloorIntake));
controller.b().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kReefIntake));
controller.x().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kScore));
controller.y().whileTrue(AlgaePivotCommands.setAlgaePivotAngle(AlgaeSubsystem.State.kStow));

}

@Override
Expand All @@ -76,7 +89,7 @@ public Translation2d getDriveTranslation() {
final var xComponent = translationStickCurve.calculate(controller.getLeftX());
final var yComponent = translationStickCurve.calculate(controller.getLeftY());
// Components are reversed because field coordinates are opposite of joystick coordinates
// System.out.println("Colin Is GAY!!! " + new Translation2d(yComponent, xComponent).toString());


return new Translation2d(yComponent, xComponent);
}
Expand All @@ -88,4 +101,6 @@ public void setDriveRotation(double requestedAngularVel) {
public double getDriveRotation() {
return rotationStickCurve.calculate(-controller.getRightX());
}

public double getRightX() { return controller.getRightX(); }
}
Loading