Skip to content

Commit

Permalink
Tested drivetrain on old robot, so it confirmed works!
Browse files Browse the repository at this point in the history
  • Loading branch information
@a1cd
a1cd committed Jan 17, 2024
1 parent 86e0ddb commit c4cdfe0
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 48 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final Mode currentMode = Mode.SIM;
public static final Mode currentMode = Mode.REAL;

public static enum Mode {
public enum Mode {
/** Running on a real robot. */
REAL,

Expand Down
86 changes: 44 additions & 42 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
import frc.robot.subsystems.flywheel.Flywheel;
import frc.robot.subsystems.flywheel.FlywheelIO;
import frc.robot.subsystems.flywheel.FlywheelIOSim;
import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

Expand Down Expand Up @@ -69,11 +68,11 @@ public RobotContainer() {
drive =
new Drive(
new GyroIOPigeon2(),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
flywheel = new Flywheel(new FlywheelIOSparkMax());
new ModuleIOTalonFX(0),
new ModuleIOTalonFX(1),
new ModuleIOTalonFX(2),
new ModuleIOTalonFX(3));
flywheel = new Flywheel(new FlywheelIOSim());
feeder = new Feeder(new FeederIO() {});
// drive = new Drive(
// new GyroIOPigeon2(),
Expand Down Expand Up @@ -133,7 +132,7 @@ public RobotContainer() {
configureButtonBindings();
}

public static SysIDMode sysIDMode = SysIDMode.Disabled;
static SysIDMode sysIDMode = SysIDMode.DriveMotors;

enum SysIDMode {
Disabled,
Expand Down Expand Up @@ -171,7 +170,9 @@ private void configureButtonBindings() {
.a()
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
() -> flywheel.runVelocity(flywheelSpeedInput.get()),
flywheel::stop,
flywheel));
break;
case DriveMotors:
drive.setDefaultCommand(
Expand All @@ -181,48 +182,49 @@ private void configureButtonBindings() {
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
var drivetrainDriveSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateDriveCharacterizationData,
drive,
"DrivetrainDriveMotors"));
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateDriveCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
.x()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.y()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
.y()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.a()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
.a()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
.b()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case TurnMotors:
var drivetrainTurnSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateTurnCharacterizationData,
drive,
"DrivetrainDriveMotors"));
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateTurnCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainTurnSysID.dynamic(Direction.kForward)
.andThen(drivetrainTurnSysID.dynamic(Direction.kReverse))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kForward))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kReverse))
.andThen(Commands.runOnce(drive::stopWithX, drive))
)
.onFalse(Commands.runOnce(drive::stopWithX, drive));
.x()
.whileTrue(
drivetrainTurnSysID
.dynamic(Direction.kForward)
.andThen(drivetrainTurnSysID.dynamic(Direction.kReverse))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kForward))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kReverse))
.andThen(Commands.runOnce(drive::stopWithX, drive)))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case EverythingElse:
break;
Expand Down
11 changes: 11 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ public class Drive extends SubsystemBase {
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final Module[] modules = new Module[4]; // FL, FR, BL, BR

@SuppressWarnings("FieldMayBeFinal")
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Pose2d pose = new Pose2d();
private Rotation2d lastGyroRotation = new Rotation2d();
Expand Down Expand Up @@ -191,39 +192,49 @@ public double getCharacterizationVelocity() {
public void populateDriveCharacterizationData(SysIdRoutineLog routineLog) {
Measure<Velocity<Angle>> driveVelocityAverage = RadiansPerSecond.zero();
Measure<Angle> drivePositionAverage = Radians.zero();
Measure<Voltage> driveVoltageAverage = Volts.zero();

for (var module : modules) {
var motor = routineLog.motor("DriveMotor #" + module.getIndex());
var angularPosition = module.getCharacterizationDrivePosition();
var angularVelocity = module.getCharacterizationDriveVelocity();
var voltage = module.getCharacterizationDriveAppliedVoltage();
motor.angularPosition(angularPosition);
motor.angularVelocity(angularVelocity);
motor.voltage(voltage);

drivePositionAverage = drivePositionAverage.plus(angularPosition);
driveVelocityAverage = driveVelocityAverage.plus(angularVelocity);
driveVoltageAverage = driveVoltageAverage.plus(voltage);
}
var averageDriveMotor = routineLog.motor("Average DriveMotor");
averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0));
averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0));
averageDriveMotor.voltage(driveVoltageAverage.divide(4.0));
}

public void populateTurnCharacterizationData(SysIdRoutineLog routineLog) {
Measure<Velocity<Angle>> driveVelocityAverage = RadiansPerSecond.zero();
Measure<Angle> drivePositionAverage = Radians.zero();
Measure<Voltage> driveVoltageAverage = Volts.zero();

for (var module : modules) {
var motor = routineLog.motor("TurnMotor #" + module.getIndex());
var angularPosition = module.getCharacterizationTurnPosition();
var angularVelocity = module.getCharacterizationTurnVelocity();
var voltage = module.getCharacterizationDriveAppliedVoltage();
motor.angularPosition(angularPosition);
motor.angularVelocity(angularVelocity);
motor.voltage(voltage);

driveVelocityAverage = driveVelocityAverage.plus(angularVelocity);
drivePositionAverage = drivePositionAverage.plus(angularPosition);
driveVoltageAverage = driveVoltageAverage.plus(voltage);
}
var averageDriveMotor = routineLog.motor("Average TurnMotor");
averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0));
averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0));
averageDriveMotor.voltage(driveVoltageAverage.divide(4.0));
}

/** Returns the module states (turn angles and drive velocities) for all of the modules. */
Expand Down
23 changes: 19 additions & 4 deletions src/main/java/frc/robot/subsystems/drive/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@

package frc.robot.subsystems.drive;

import static edu.wpi.first.units.Units.*;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -22,6 +24,7 @@
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import frc.robot.Constants;
import org.littletonrobotics.junction.Logger;

Expand All @@ -48,6 +51,10 @@ public Module(ModuleIO io, int index) {
// separate robot with different tuning)
switch (Constants.currentMode) {
case REAL:
driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
driveFeedback = new PIDController(0.05, 0.0, 0.0);
turnFeedback = new PIDController(7.0, 0.0, 0.0);
break;
case REPLAY:
driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
driveFeedback = new PIDController(0.05, 0.0, 0.0);
Expand Down Expand Up @@ -186,19 +193,27 @@ public double getCharacterizationVelocityRadPerSec() {

/** Returns the drive velocity unitless. */
public Measure<Angle> getCharacterizationDrivePosition() {
return edu.wpi.first.units.Units.Radians.of(inputs.drivePositionRad);
return Radians.of(inputs.drivePositionRad);
}
/** Returns the turn velocity unitless. */
public Measure<Angle> getCharacterizationTurnPosition() {
return edu.wpi.first.units.Units.Radians.of(inputs.turnPosition.getRadians());
return Radians.of(inputs.turnPosition.getRadians());
}
/** Returns the drive velocity unitless. */
public Measure<Velocity<Angle>> getCharacterizationDriveVelocity() {
return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.driveVelocityRadPerSec);
return RadiansPerSecond.of(inputs.driveVelocityRadPerSec);
}
/** Returns the turn velocity unitless. */
public Measure<Velocity<Angle>> getCharacterizationTurnVelocity() {
return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.turnVelocityRadPerSec);
return RadiansPerSecond.of(inputs.turnVelocityRadPerSec);
}

public Measure<Voltage> getCharacterizationDriveAppliedVoltage() {
return Volts.of(inputs.driveAppliedVolts);
}

public Measure<Voltage> getCharacterizationTurnAppliedVoltage() {
return Volts.of(inputs.turnAppliedVolts);
}

public int getIndex() {
Expand Down

0 comments on commit c4cdfe0

Please sign in to comment.