Skip to content

Commit

Permalink
simulated tuning
Browse files Browse the repository at this point in the history
  • Loading branch information
@rishabhreng
rishabhreng committed Dec 28, 2024
1 parent 4751ea2 commit da79134
Show file tree
Hide file tree
Showing 13 changed files with 217 additions and 165 deletions.
32 changes: 14 additions & 18 deletions AdvKit2024/src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,23 +75,23 @@ public class DriveConstants {
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains =
new Slot0Configs()
.withKP(100)
.withKP(50)
.withKI(0)
.withKD(0.2)
.withKS(0)
.withKV(1.5)
.withKA(0)
.withKD(0.0)
.withKS(0.03428)
.withKV(0.22857)
.withKA(0.0014722)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains =
new Slot0Configs()
.withKP(0.17029)
.withKP(0.17527)
.withKI(0)
.withKD(0)
.withKS(0.06650)
.withKV(0.71338)
.withKA(0.077262);
.withKS(0.066052)
.withKV(0.11653)
.withKA(0.014691);

// The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors
Expand Down Expand Up @@ -131,7 +131,7 @@ public class DriveConstants {

// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6.21);
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(7.0);

// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
Expand Down Expand Up @@ -352,14 +352,10 @@ public static class PivotConstants {
public static final int PIVOT_MAIN_MOTOR_ID = 1;
public static final int PIVOT_FOLLOWER_MOTOR_ID = 5;

public static final double PIVOT_ARM_INIT_POSE = 37;

public static final double PIVOT_OFFSET = 0.307506;

public static final double PIVOT_kV = 0;
public static final double PIVOT_kP = 8;
public static final double PIVOT_kD = 0.01;
public static final double PIVOT_kI = 0;
public static final double PIVOT_kV = 9.0;
public static final double PIVOT_kP = 12.0;
public static final double PIVOT_kI = 4.0;
public static final double PIVOT_kD = 2.0;

// motion magic constraints
public static final double PIVOT_CRUISE_VELOCITY = 2500;
Expand Down
22 changes: 16 additions & 6 deletions AdvKit2024/src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,10 @@ public RobotContainer() {
new VisionIOPhotonVisionSim(
CAM_0_NAME, robotToCamera0, driveSimulation::getSimulatedDriveTrainPose));

intake = new Intake(new IntakeIOSim());
shooter = new Shooter(new ShooterIOSim());
intake =
new Intake(
new IntakeIOSim(driveSimulation, () -> shooter.getState() != ShooterState.IDLE));
pivot = new Pivot(new PivotIOSim());
index = new Index(new IndexIOSim());
break;
Expand Down Expand Up @@ -192,8 +194,7 @@ private void configureButtonBindings() {
? () ->
drive.setPose(
driveSimulation
.getSimulatedDriveTrainPose()) // reset odometry to actual robot pose during
// simulation
.getSimulatedDriveTrainPose()) // reset to actual pose during simulation
: () ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro
Expand All @@ -205,12 +206,14 @@ private void configureButtonBindings() {
// intake mechanism
controller
.rightTrigger()
.onTrue(
.whileTrue(
Commands.parallel(
shooter.setState(ShooterState.IDLE),
pivot.setState(PivotState.INTAKE),
intake.setState(IntakeState.INTAKE),
index.setState(IndexState.INTAKE)));
index.setState(IndexState.INTAKE)))
.onFalse(
Commands.parallel(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)));
intake.noteInIntake.onTrue(
Commands.parallel(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)));

Expand All @@ -224,6 +227,13 @@ private void configureButtonBindings() {
controller.leftBumper().onTrue(pivot.setState(PivotState.AMP));
controller.x().onTrue(pivot.setState(PivotState.SHUTTLE));

controller
.start()
.onTrue(
Commands.runOnce(
() ->
driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d()))));

// hold to rev shooter, let go to shoot
controller
.leftTrigger()
Expand Down Expand Up @@ -253,7 +263,7 @@ public Command getAutonomousCommand() {
public void resetSimulationField() {
if (Constants.currentMode != Constants.Mode.SIM) return;

driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d()));
// driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d()));
SimulatedArena.getInstance().resetFieldForAuto();
}

Expand Down
25 changes: 14 additions & 11 deletions AdvKit2024/src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,19 +85,20 @@ public static Command joystickDrive(
omega = Math.copySign(omega * omega, omega);

// Convert to field relative speeds & send command
ChassisSpeeds speeds =
new ChassisSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec());
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;

ChassisSpeeds speeds =
speeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
speeds,
isFlipped
? drive.getRotation().plus(new Rotation2d(Math.PI))
: drive.getRotation());

drive.runVelocity(speeds);
},
drive);
Expand Down Expand Up @@ -136,15 +137,17 @@ public static Command joystickDriveAtAngle(
drive.getRotation().getRadians(), rotationSupplier.get().getRadians());

// Convert to field relative speeds & send command
ChassisSpeeds speeds =
new ChassisSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega);
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;

ChassisSpeeds speeds =
speeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
speeds,
isFlipped
? drive.getRotation().plus(new Rotation2d(Math.PI))
: drive.getRotation());
Expand Down
6 changes: 3 additions & 3 deletions AdvKit2024/src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,8 +177,8 @@ public Drive(
new SysIdRoutine(
new SysIdRoutine.Config(
null,
null,
null,
Volts.of(3.0),
Seconds.of(7.0),
(state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
new SysIdRoutine.Mechanism(
(voltage) -> runCharacterization(voltage.in(Volts)), null, this));
Expand Down Expand Up @@ -250,7 +250,7 @@ public void periodic() {
*/
public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
ChassisSpeeds.discretize(speeds, 0.02);
speeds = ChassisSpeeds.discretize(speeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.kSpeedAt12Volts);

Expand Down
10 changes: 6 additions & 4 deletions AdvKit2024/src/main/java/frc/robot/subsystems/drive/module/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ public void periodic() {
public void runSetpoint(SwerveModuleState state) {
// Optimize velocity setpoint
state.optimize(getAngle());
state.cosineScale(inputs.turnPosition);
state.cosineScale(inputs.turnAbsolutePosition);

// Apply setpoints
io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius);
Expand All @@ -82,8 +82,10 @@ public void runSetpoint(SwerveModuleState state) {

/** Runs the module with the specified output while controlling to zero degrees. */
public void runCharacterization(double output) {
io.setDriveOpenLoop(output);
io.setTurnPosition(new Rotation2d());
// io.setDriveOpenLoop(output);
// io.setTurnPosition(new Rotation2d());
io.setDriveOpenLoop(0.0);
io.setTurnOpenLoop(output);
}

/** Disables all outputs to motors. */
Expand All @@ -94,7 +96,7 @@ public void stop() {

/** Returns the current turn angle of the module. */
public Rotation2d getAngle() {
return inputs.turnPosition;
return inputs.turnAbsolutePosition;
}

/** Returns the current drive position of the module in meters. */
Expand Down
2 changes: 1 addition & 1 deletion AdvKit2024/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ public static class ModuleIOInputs {

public boolean turnConnected = false;
public boolean turnEncoderConnected = false;
public Rotation2d turnAbsolutePosition = new Rotation2d();
public Rotation2d turnPosition = new Rotation2d();
public Rotation2d turnAbsolutePosition = new Rotation2d();
public double turnVelocityRadPerSec = 0.0;
public double turnAppliedVolts = 0.0;
public double turnCurrentAmps = 0.0;
Expand Down
29 changes: 18 additions & 11 deletions AdvKit2024/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,9 @@
package frc.robot.subsystems.drive.module;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static frc.robot.util.PhoenixUtil.tryUntilOk;

import com.ctre.phoenix6.BaseStatusSignal;
Expand Down Expand Up @@ -51,6 +55,7 @@ public abstract class ModuleIOTalonFX implements ModuleIO {
protected final StatusSignal<Current> driveCurrent;

// Inputs from turn motor
protected final StatusSignal<Angle> turnPosition;
protected final StatusSignal<Angle> turnAbsolutePosition;
protected final StatusSignal<AngularVelocity> turnVelocity;
protected final StatusSignal<Voltage> turnAppliedVolts;
Expand Down Expand Up @@ -127,6 +132,7 @@ protected ModuleIOTalonFX(SwerveModuleConstants constants) {

// Create turn status signals
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnPosition = turnTalon.getPosition();
turnVelocity = turnTalon.getVelocity();
turnAppliedVolts = turnTalon.getMotorVoltage();
turnCurrent = turnTalon.getStatorCurrent();
Expand All @@ -139,6 +145,7 @@ protected ModuleIOTalonFX(SwerveModuleConstants constants) {
driveVelocity,
driveAppliedVolts,
driveCurrent,
turnPosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);
Expand All @@ -150,25 +157,25 @@ public void updateInputs(ModuleIOInputs inputs) {
// Refresh all signals
var driveStatus =
BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
var turnStatus = BaseStatusSignal.refreshAll(turnVelocity, turnAppliedVolts, turnCurrent);
var turnStatus =
BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);

// Update drive inputs
inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
inputs.drivePositionRad =
Units.rotationsToRadians(drivePosition.getValueAsDouble()) / constants.DriveMotorGearRatio;
inputs.driveVelocityRadPerSec =
Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / constants.DriveMotorGearRatio;
inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
inputs.drivePositionRad = drivePosition.getValue().in(Radians);
inputs.driveVelocityRadPerSec = driveVelocity.getValue().in(RadiansPerSecond);
inputs.driveAppliedVolts = driveAppliedVolts.getValue().in(Volts);
inputs.driveCurrentAmps = driveCurrent.getValue().in(Amps);

// Update turn inputs
inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble());
inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
inputs.turnPosition = new Rotation2d(turnPosition.getValue());
inputs.turnAbsolutePosition = new Rotation2d(turnAbsolutePosition.getValue());
inputs.turnVelocityRadPerSec = turnVelocity.getValue().in(RadiansPerSecond);
inputs.turnAppliedVolts = turnAppliedVolts.getValue().in(Volts);
inputs.turnCurrentAmps = turnCurrent.getValue().in(Amps);
}

@Override
Expand Down
Loading

0 comments on commit da79134

Please sign in to comment.