Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
feat: shooter ffs, swerve, other stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@WeilSimon
WeilSimon committed Dec 14, 2023
1 parent 7a4573f commit 8d7ae9f
Show file tree
Hide file tree
Showing 10 changed files with 182 additions and 94 deletions.
12 changes: 8 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,13 +37,17 @@ public static enum Mode {

public static class Drivetrain{
public static final double[] offsets = new double[]{
75.469, // Module 1
// .917725 * 360, // Module 1
75.469 + 108 - 180 - 3 - 27 + 90,
91.318 - 90, // Module 2
256.729 + 3, // Module 3
9.0, // Module 4
// .224365 * 360, // Module 3
259.729 + 30 - 180 - 1 -27 + 90,
// .022949 * 360, // Module 4
9.0 + 30 -27 + 90,
239.766, // Module 5
32.08, // Module 6
27.861, // Module 7
// -.423096 * 360, // Module 7
27.861 + 120 - 27 + 90,
105.011 + 3 // Module 8
};

Expand Down
154 changes: 84 additions & 70 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
Expand Down Expand Up @@ -54,80 +55,64 @@ public class RobotContainer {
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
switch (Constants.currentMode) {
case REAL:
case REAL: {
// Real robot, instantiate hardware IO implementations
// drive =
// new Drive(
// new GyroIONavx(),
// new ModuleIOTalonFX(0, 3),
// new ModuleIOTalonFX(1, 4),
// new ModuleIOTalonFX(2, 7),
// new ModuleIOTalonFX(3, 1));
drive = null;
drive =
new Drive(
new GyroIONavx(),
new ModuleIOTalonFX(0, 3),
new ModuleIOTalonFX(1, 4),
new ModuleIOTalonFX(2, 7),
new ModuleIOTalonFX(3, 1));
// drive = null;
shooter =
new Shooter(
new FlywheelIOTalonFX(),
new FlywheelIOSparkMaxSingle(1,13),
new FlywheelIOSparkMaxSingle(10,14));
new Shooter(
new FlywheelIOTalonFX(),
new FlywheelIOSparkMaxSingle(1, 13, false, 20),
new FlywheelIOSparkMaxSingle(10, 14, true, 40));
limelight = new Limelight(new LimelightIOReal());
// drive = new Drive(
// new GyroIOPigeon2(),
// new ModuleIOTalonFX(0),
// new ModuleIOTalonFX(1),
// new ModuleIOTalonFX(2),
// new ModuleIOTalonFX(3));
break;
}

case SIM: {

case SIM:
// Sim robot, instantiate physics sim IO implementations
drive =
new Drive(
new GyroIO() {},
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
new Drive(
new GyroIO() {
},
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
shooter = null;
limelight = null;
break;
}

default:
default: {
// Replayed robot, disable IO implementations
drive =
new Drive(
new GyroIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {});
new Drive(
new GyroIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
});
shooter = null;
limelight = null;
break;
}
}

// Set up named commands for PathPlanner

// Set up auto routines
// Set up named commands for PathPlanner
autoChooser = new LoggedDashboardChooser<>("Auto Choices");
configureAutoChooser();

// Set up FF characterization routines
// autoChooser.addOption(
// "Drive FF Characterization",
// new FeedForwardCharacterization(
// drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity));

autoChooser.addOption(
"Shooter Main FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runMainCharacterizationVolts,
shooter::getMainCharacterizationVelocity));
autoChooser.addOption(
"Shooter Secondary FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runSecondaryCharacterizationVolts,
shooter::getSecondaryCharacterizationVelocity));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -140,15 +125,17 @@ public RobotContainer() {
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// drive.setDefaultCommand(
// DriveCommands.joystickDrive(
// drive,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// () -> -controller.getRightX()));

shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter));
// controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftX(),
() -> controller.getLeftY(),
() -> -controller.getRightX()));

// shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter));
shooter.setDefaultCommand(new ShooterCommands.ShooterTesting(shooter));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller.y().onTrue(new InstantCommand(() -> drive.resetPose()));
// controller
// .b()
// .onTrue(
Expand All @@ -159,13 +146,40 @@ private void configureButtonBindings() {
// drive)
// .ignoringDisable(true));

// controller.x()
// .whileTrue(DriveCommands.LimelightRotDrive(
// drive,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// limelight))
controller.a()
.whileTrue(DriveCommands.LimelightRotDrive(
drive,
() -> -controller.getLeftX(),
() -> controller.getLeftY(),
limelight));
// .whileTrue(new ShooterCommands.Shoot(shooter,limelight));
// controller.x().whileTrue(new ShooterCommands.ShooterTesting(shooter));
}

private void configureAutoChooser(){
// Set up auto routines

// Set up FF characterization routines
autoChooser.addOption(
"Drive FF Characterization",
new FeedForwardCharacterization(
drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity));

autoChooser.addOption(
"Shooter Main FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runMainCharacterizationVolts,
shooter::getMainCharacterizationVelocity));
autoChooser.addOption(
"Shooter Secondary FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runSecondaryCharacterizationVolts,
shooter::getSecondaryCharacterizationVelocity));
autoChooser.addOption(
"Justin Case",
new DriveCommands.JustinCase(drive));
}

/**
Expand Down
22 changes: 19 additions & 3 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,20 +14,21 @@
package frc.robot.commands;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.*;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.limelight.Limelight;

import java.util.function.DoubleSupplier;

public class DriveCommands {
private static final double DEADBAND = 0.1;
private static PIDController pid = new PIDController(0,0,0);

private DriveCommands() {}

Expand Down Expand Up @@ -77,6 +78,7 @@ public static Command LimelightRotDrive(
Limelight limelight) {
return Commands.run(
() -> {

// Apply deadband
double linearMagnitude =
MathUtil.applyDeadband(
Expand All @@ -99,9 +101,23 @@ public static Command LimelightRotDrive(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
pid.calculate(omega, 0) * drive.getMaxAngularSpeedRadPerSec(),
// omega * drive.getMaxAngularSpeedRadPerSec(),

drive.getRotation()));
},
drive);
}

public static class JustinCase extends SequentialCommandGroup {
public JustinCase(Drive drive){
addCommands(
new ParallelRaceGroup(
new WaitCommand(1),
joystickDrive(drive, () -> 0.0, () -> 0.5, () -> 0.0)
),
joystickDrive(drive, () -> 0.0, () -> 0.0, () -> 0.0)
);
}
}
}
37 changes: 33 additions & 4 deletions src/main/java/frc/robot/commands/ShooterCommands.java
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.IndexTemp;
import frc.robot.subsystems.limelight.Limelight;
import frc.robot.subsystems.shooter.Shooter;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

public class ShooterCommands {
public static class Shoot extends CommandBase {
Expand Down Expand Up @@ -66,9 +67,9 @@ public boolean isFinished() {
public static class ShooterIdle extends CommandBase {
Shooter shooter;

private final double mainIdleRPM = 8000;
private final double secondaryIdleRPM = 4000;
private final double loadingIdleVoltage = -8;
private final double mainIdleRPM = 4000;
private final double secondaryIdleRPM = 0;
private final double loadingIdleVoltage = 0;

public ShooterIdle(Shooter shooter) {
this.shooter = shooter;
Expand All @@ -82,4 +83,32 @@ public void initialize() {
shooter.setLoadingVoltage(loadingIdleVoltage);
}
}

public static class ShooterTesting extends CommandBase{
Shooter shooter;
LoggedDashboardNumber mainRPM = new LoggedDashboardNumber("mainRPM", 0);
LoggedDashboardNumber secondaryRPM = new LoggedDashboardNumber("secondaryRPM", 0);

LoggedDashboardNumber loadingVoltage = new LoggedDashboardNumber("loadingVoltage",0);
LoggedDashboardNumber indexPercent = new LoggedDashboardNumber("indexPercent",0);
IndexTemp index = new IndexTemp();
public ShooterTesting(Shooter shooter) {
this.shooter = shooter;
addRequirements(shooter);
}

@Override
public void execute() {
shooter.setMainVelocity(mainRPM.get());
shooter.setSecondaryVelocity(secondaryRPM.get());
shooter.setLoadingVoltage(loadingVoltage.get());
index.setPercent(indexPercent.get());
}

@Override
public void end(boolean interrupted) {
shooter.stopAll();
index.setPercent(0);
}
}
}
19 changes: 19 additions & 0 deletions src/main/java/frc/robot/subsystems/IndexTemp.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

public class IndexTemp extends SubsystemBase {
CANSparkMax spark = new CANSparkMax(15, CANSparkMaxLowLevel.MotorType.kBrushless);

public void setPercent(double percent){
spark.set(percent);
}

@Override
public void periodic() {
Logger.getInstance().recordOutput("Indexer/outputCurrent", spark.getOutputCurrent());
}
}
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,8 @@

public class Drive extends SubsystemBase {
private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
private static final double TRACK_WIDTH_X = Units.inchesToMeters(21.0);
private static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.0);
private static final double DRIVE_BASE_RADIUS =
Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
Expand Down Expand Up @@ -221,6 +221,10 @@ public void setPose(Pose2d pose) {
this.pose = pose;
}

public void resetPose(){
this.pose = new Pose2d(pose.getX(), pose.getY(), Rotation2d.fromDegrees(-90));
}

/** Returns the maximum linear speed in meters per sec. */
public double getMaxLinearSpeedMetersPerSec() {
return MAX_LINEAR_SPEED;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ public ModuleIOTalonFX(int index, int moduleNumber) {
driveTalon = new TalonFX(moduleNumber + 30, "swerve");
turnTalon = new TalonFX(moduleNumber + 20, "swerve");
cancoder = new CANcoder(moduleNumber + 10, "swerve");
absoluteEncoderOffset = new Rotation2d((Constants.Drivetrain.cornerOffsets[index] + Constants.Drivetrain.offsets[moduleNumber])%360); // MUST BE CALIBRATED
absoluteEncoderOffset = Rotation2d.fromDegrees((Constants.Drivetrain.cornerOffsets[index] + Constants.Drivetrain.offsets[moduleNumber-1]) % 360); // MUST BE CALIBRATED

var driveConfig = new TalonFXConfiguration();
driveConfig.CurrentLimits.StatorCurrentLimit = 40.0;
Expand Down
Loading

0 comments on commit 8d7ae9f

Please sign in to comment.