Skip to content

Commit

Permalink
Merge branch 'robototes:main' into Logging
Browse files Browse the repository at this point in the history
  • Loading branch information
@kirbt
kirbt authored Mar 19, 2024
2 parents 41212bb + e12a6cc commit 06422b4
Show file tree
Hide file tree
Showing 8 changed files with 433 additions and 146 deletions.
27 changes: 27 additions & 0 deletions src/main/java/frc/team2412/robot/Controls.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.team2412.robot.commands.intake.AllInCommand;
import frc.team2412.robot.commands.intake.AllReverseCommand;
import frc.team2412.robot.commands.intake.AllStopCommand;
Expand Down Expand Up @@ -72,6 +73,10 @@ public Controls(Subsystems s) {
codriveIntakeReverseButton = codriveController.povLeft();
codriveIntakeRejectButton = codriveController.povDown();

if (Robot.isSysIdMode() && LAUNCHER_ENABLED) {
bindSysIdControls();
return;
}
if (DRIVEBASE_ENABLED) {
bindDrivebaseControls();
}
Expand Down Expand Up @@ -175,6 +180,28 @@ private void bindLauncherControls() {
driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(4000)));
}

private void bindSysIdControls() {
// only one routine can be run in one robot log
// switch these between arm and flywheel in code when tuning
driveController
.leftBumper()
.whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kForward));
driveController
.rightBumper()
.whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kReverse));
driveController
.leftTrigger()
.whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kForward));
driveController
.rightTrigger()
.whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kReverse));
// switch these between angle and drive tests in code when tuning
driveController.x().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kForward));
driveController.y().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kReverse));
driveController.a().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kForward));
driveController.b().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kReverse));
}

public void vibrateDriveController(double vibration) {
if (!DriverStation.isAutonomous()) {
driveController.getHID().setRumble(RumbleType.kBothRumble, vibration);
Expand Down
9 changes: 9 additions & 0 deletions src/main/java/frc/team2412/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.team2412.robot;

import com.ctre.phoenix6.SignalLogger;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
Expand Down Expand Up @@ -40,6 +41,8 @@ public static Robot getInstance() {
}

private static final boolean debugMode = true;
// Really dangerous to keep this enabled as it disables all other controls, use with caution
private static final boolean sysIdMode = false;

private final RobotType robotType;
private final PowerDistribution PDP;
Expand Down Expand Up @@ -148,6 +151,7 @@ public void autonomousInit() {
@Override
public void teleopInit() {
Shuffleboard.startRecording();
SignalLogger.start();
}

@Override
Expand All @@ -158,6 +162,7 @@ public void autonomousExit() {
@Override
public void teleopExit() {
CommandScheduler.getInstance().cancelAll();
SignalLogger.stop();
}

@Override
Expand Down Expand Up @@ -192,4 +197,8 @@ public boolean isCompetition() {
public static boolean isDebugMode() {
return debugMode;
}

public static boolean isSysIdMode() {
return sysIdMode;
}
}
47 changes: 47 additions & 0 deletions src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem;
Expand All @@ -21,6 +23,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.team2412.robot.Robot;
import frc.team2412.robot.Robot.RobotType;
import frc.team2412.robot.Subsystems.SubsystemConstants;
Expand All @@ -30,6 +33,7 @@
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import swervelib.SwerveDrive;
import swervelib.SwerveModule;
import swervelib.math.SwerveMath;
import swervelib.parser.SwerveParser;
import swervelib.telemetry.SwerveDriveTelemetry;
Expand Down Expand Up @@ -319,4 +323,47 @@ private void initShuffleboard() {
public SwerveDrive getSwerveDrive() {
return swerveDrive;
}

private SysIdRoutine getDriveSysIdRoutine() {
return new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
for (SwerveModule module : swerveDrive.getModules()) {
module.getDriveMotor().setVoltage(volts.magnitude());
module.setAngle(0);
}
},
null,
this));
}

private SysIdRoutine getAngleSysIdRoutine() {
return new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
for (SwerveModule module : swerveDrive.getModules()) {
module.getAngleMotor().setVoltage(volts.magnitude());
}
},
null,
this));
}

public Command driveSysIdQuasistatic(SysIdRoutine.Direction direction) {
return getDriveSysIdRoutine().quasistatic(direction);
}

public Command driveSysIdDynamic(SysIdRoutine.Direction direction) {
return getDriveSysIdRoutine().dynamic(direction);
}

public Command angleSysIdQuasistatic(SysIdRoutine.Direction direction) {
return getAngleSysIdRoutine().quasistatic(direction);
}

public Command angleSysIdDynamic(SysIdRoutine.Direction direction) {
return getAngleSysIdRoutine().dynamic(direction);
}
}
134 changes: 114 additions & 20 deletions src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,19 @@
import com.revrobotics.SparkAbsoluteEncoder.Type;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.units.BaseUnits;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.team2412.robot.Hardware;
import frc.team2412.robot.Robot;
import frc.team2412.robot.util.SparkPIDWidget;
Expand Down Expand Up @@ -52,10 +60,18 @@ public class LauncherSubsystem extends SubsystemBase {
private final RelativeEncoder launcherBottomEncoder;
private final SparkAbsoluteEncoder launcherAngleEncoder;
private final SparkPIDController launcherAngleOnePIDController;
// private final SparkPIDController launcherAngleTwoPIDController;

// arm FF values:
// Ks: 0.40434
// Kv: 0.096771
// Ka: 0.0056403

private final SparkPIDController launcherTopPIDController;
private final SparkPIDController launcherBottomPIDController;
private final SimpleMotorFeedforward launcherTopFeedforward =
new SimpleMotorFeedforward(0, 0.11335, 0.048325);
private final SimpleMotorFeedforward launcherBottomFeedforward =
new SimpleMotorFeedforward(0, 0.11238, 0.048209);

private double rpmSetpoint;
private double angleSetpoint;
Expand Down Expand Up @@ -129,26 +145,18 @@ public void configMotors() {
launcherAngleTwoMotor.follow(launcherAngleOneMotor, true);

// PID
launcherAngleOnePIDController.setP(5.0);
launcherAngleOnePIDController.setP(5.421);
launcherAngleOnePIDController.setI(0);
launcherAngleOnePIDController.setD(0);
launcherAngleOnePIDController.setFF(0);
launcherAngleOnePIDController.setD(0.066248);
launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED);

// launcherAngleTwoPIDController.setP(0.1);
// launcherAngleTwoPIDController.setI(0);
// launcherAngleTwoPIDController.setD(0);
// launcherAngleTwoPIDController.setFF(0);

launcherTopPIDController.setP(0.002);
launcherTopPIDController.setP(7.7633E-05);
launcherTopPIDController.setI(0);
launcherTopPIDController.setD(0.001);
launcherTopPIDController.setFF(0);
launcherTopPIDController.setD(0);

launcherBottomPIDController.setP(0.002);
launcherBottomPIDController.setP(0.00011722);
launcherBottomPIDController.setI(0);
launcherBottomPIDController.setD(0.001);
launcherBottomPIDController.setFF(0);
launcherBottomPIDController.setD(0);

launcherAngleOneMotor.getEncoder().setPosition(launcherAngleEncoder.getPosition());
launcherAngleOneMotor.getEncoder().setPositionConversionFactor(PIVOT_GEARING_RATIO);
Expand All @@ -169,18 +177,20 @@ public void stopLauncher() {
// uses the value from the entry
public void launch() {
rpmSetpoint = setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM);
launcherTopPIDController.setReference(rpmSetpoint, ControlType.kVelocity);
launcherBottomPIDController.setReference(rpmSetpoint, ControlType.kVelocity);
launch(rpmSetpoint);
}
// used for presets
public void launch(double speed) {
rpmSetpoint = speed;
launcherTopPIDController.setReference(rpmSetpoint, ControlType.kVelocity);
launcherBottomPIDController.setReference(rpmSetpoint, ControlType.kVelocity);
launcherTopPIDController.setReference(
rpmSetpoint, ControlType.kVelocity, 0, launcherTopFeedforward.calculate(speed));
launcherBottomPIDController.setReference(
rpmSetpoint, ControlType.kVelocity, 0, launcherBottomFeedforward.calculate(speed));
}

public void ampLaunch(double speed) {
launcherTopPIDController.setReference(-speed, ControlType.kVelocity);
launcherTopPIDController.setReference(
-speed, ControlType.kVelocity, 0, launcherTopFeedforward.calculate(-speed));
launcherBottomMotor.disable();
}

Expand Down Expand Up @@ -292,4 +302,88 @@ public void periodic() {
launcherAngleSpeedEntry.setDouble(getAngleSpeed());
launcherIsAtSpeed.setBoolean(isAtSpeed());
}

private SysIdRoutine getArmSysIdRoutine() {
return new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
launcherAngleOneMotor.setVoltage(volts.in(BaseUnits.Voltage));
launcherAngleTwoMotor.setVoltage(volts.in(BaseUnits.Voltage));
},
(SysIdRoutineLog log) -> {
log.motor("angle-one")
.voltage(
BaseUnits.Voltage.of(
launcherAngleOneMotor.getAppliedOutput()
* RobotController.getBatteryVoltage()))
.angularPosition(
edu.wpi.first.units.Units.Rotations.of(
launcherAngleOneMotor.getEncoder().getPosition()))
.angularVelocity(
edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute)
.of(launcherAngleOneMotor.getEncoder().getVelocity()));
log.motor("angle-two")
.voltage(
BaseUnits.Voltage.of(
launcherAngleTwoMotor.getAppliedOutput()
* RobotController.getBatteryVoltage()))
.angularPosition(
edu.wpi.first.units.Units.Rotations.of(
launcherAngleTwoMotor.getEncoder().getPosition()))
.angularVelocity(
edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute)
.of(launcherAngleTwoMotor.getEncoder().getVelocity()));
},
this));
}

private SysIdRoutine getFlywheelSysIdRoutine() {
return new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
launcherTopMotor.setVoltage(volts.in(BaseUnits.Voltage));
launcherBottomMotor.setVoltage(volts.in(BaseUnits.Voltage));
},
(SysIdRoutineLog log) -> {
log.motor("top-flywheel")
.voltage(
BaseUnits.Voltage.of(
launcherTopMotor.getAppliedOutput()
* RobotController.getBatteryVoltage()))
.angularPosition(
edu.wpi.first.units.Units.Rotations.of(launcherTopEncoder.getPosition()))
.angularVelocity(
edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute)
.of(launcherTopEncoder.getVelocity()));
log.motor("bottom-flywheel")
.voltage(
BaseUnits.Voltage.of(
launcherBottomMotor.getAppliedOutput()
* RobotController.getBatteryVoltage()))
.angularPosition(
edu.wpi.first.units.Units.Rotations.of(launcherBottomEncoder.getPosition()))
.angularVelocity(
edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute)
.of(launcherBottomEncoder.getVelocity()));
},
this));
}

public Command armSysIdQuasistatic(SysIdRoutine.Direction direction) {
return getArmSysIdRoutine().quasistatic(direction);
}

public Command armSysIdDynamic(SysIdRoutine.Direction direction) {
return getArmSysIdRoutine().dynamic(direction);
}

public Command flywheelSysIdQuasistatic(SysIdRoutine.Direction direction) {
return getFlywheelSysIdRoutine().quasistatic(direction);
}

public Command flywheelSysIdDynamic(SysIdRoutine.Direction direction) {
return getFlywheelSysIdRoutine().dynamic(direction);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/team2412/robot/util/AutoLogic.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ public void registerCommands() {
"MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1);
NamedCommands.registerCommand(
"LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3);
NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4);
NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4);
NamedCommands.registerCommand(
"TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1);
NamedCommands.registerCommand(
Expand Down
Loading

0 comments on commit 06422b4

Please sign in to comment.