Skip to content

Commit

Permalink
fix: remove most sysid stuff
Browse files Browse the repository at this point in the history 
Only the shooter one was used

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
  • Loading branch information
@spacey-sooty
spacey-sooty committed Oct 8, 2024
1 parent 94120ee commit a5cc7cd
Show file tree
Hide file tree
Showing 4 changed files with 0 additions and 221 deletions.
45 changes: 0 additions & 45 deletions src/main/java/frc/robot/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,34 +4,21 @@

package frc.robot.subsystems;

import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Volts;

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.util.datalog.DataLog;
import edu.wpi.first.util.datalog.DoubleLogEntry;
import edu.wpi.first.util.datalog.StringLogEntry;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
import java.util.function.DoubleSupplier;

Expand Down Expand Up @@ -77,30 +64,6 @@ public enum Setpoint {
public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
private double m_lastPosition = 0.2;

private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));

private final SysIdRoutine m_sysIdRoutine =
new SysIdRoutine(
new SysIdRoutine.Config(Volts.of(0.5).per(Second.of(1)), Volts.of(1), Second.of(4)),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
m_primaryMotor.setVoltage(volts.in(Volts));
},
log -> {
log.motor("arm")
.voltage(
m_appliedVoltage.mut_replace(
m_primaryMotor.get() * RobotController.getBatteryVoltage(), Volts))
.angularPosition(
m_angle.mut_replace(m_encoder.getAbsolutePosition(), Rotations))
.angularVelocity(
m_velocity.mut_replace(
(m_encoder.get() * 2 * Math.PI / 60), RotationsPerSecond));
},
this));

/** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
public Arm() {
m_followerMotor.follow(m_primaryMotor);
Expand Down Expand Up @@ -278,12 +241,4 @@ public void periodic() {
public double getAngle() {
return m_encoder.getAbsolutePosition() * 2 * Math.PI;
}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.quasistatic(direction);
}

public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
}
}
42 changes: 0 additions & 42 deletions src/main/java/frc/robot/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,27 +4,15 @@

package frc.robot.subsystems;

import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.util.datalog.DoubleLogEntry;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;

/** Our Crescendo climber Subsystem */
Expand All @@ -38,28 +26,6 @@ public class Climber extends SubsystemBase {
private final DoubleLogEntry log_pid_output =
new DoubleLogEntry(DataLogManager.getLog(), "/climber/pid/output");

private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));

private final SysIdRoutine m_sysIdRoutine =
new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
m_motor.setVoltage(volts.in(Volts));
},
log -> {
log.motor("climber")
.voltage(
m_appliedVoltage.mut_replace(
m_motor.get() * RobotController.getBatteryVoltage(), Volts))
.angularPosition(m_angle.mut_replace(m_encoder.getPosition(), Rotations))
.angularVelocity(
m_velocity.mut_replace(m_encoder.getVelocity(), RotationsPerSecond));
},
this));

/** Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
public Climber() {}

Expand All @@ -77,12 +43,4 @@ public Command climb() {
m_motor.setVoltage(output);
});
}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.quasistatic(direction);
}

public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
}
}
101 changes: 0 additions & 101 deletions src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,9 @@

package frc.robot.subsystems;

import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;

import com.choreo.lib.Choreo;
import com.choreo.lib.ChoreoControlFunction;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
Expand All @@ -30,11 +24,6 @@
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Notifier;
Expand All @@ -43,13 +32,9 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.LimelightHelpers;
import frc.robot.generated.TunerConstants;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.atomic.AtomicReference;
import java.util.function.Function;
import java.util.function.Supplier;

/**
Expand Down Expand Up @@ -90,9 +75,6 @@ public enum RotationSetpoint {
/* Keep track if we've ever applied the operator perspective before or not */
private boolean hasAppliedOperatorPerspective;

private final SysIdRoutine[] m_driveSysIdRoutines = new SysIdRoutine[4];
private final SysIdRoutine[] m_steerSysIdRoutines = new SysIdRoutine[4];

public CommandSwerveDrivetrain(
SwerveDrivetrainConstants driveTrainConstants,
double OdometryUpdateFrequency,
Expand Down Expand Up @@ -131,12 +113,6 @@ public CommandSwerveDrivetrain(
m_swerveController =
Choreo.choreoSwerveController(m_xController, m_yController, m_rotationController);

for (int i = 0; i < 4; i++) {
var module = getModule(i);
m_driveSysIdRoutines[i] = createSysIdRoutine(module.getDriveMotor());
m_steerSysIdRoutines[i] = createSysIdRoutine(module.getSteerMotor());
}

configurePathPlanner();
}

Expand Down Expand Up @@ -171,27 +147,6 @@ public ChassisSpeeds getCurrentRobotChassisSpeeds() {
return m_kinematics.toChassisSpeeds(getState().ModuleStates);
}

private SysIdRoutine createSysIdRoutine(TalonFX motor) {
MutableMeasure<Voltage> appliedVoltage = mutable(Volts.of(0));
MutableMeasure<Angle> angle = mutable(Rotations.of(0));
MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));

return new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> motor.setVoltage(volts.in(Volts)),
log ->
log.motor("swerveMotor" + motor.hashCode())
.voltage(
appliedVoltage.mut_replace(
motor.get() * RobotController.getBatteryVoltage(), Volts))
.angularPosition(angle.mut_replace(motor.getPosition().getValue(), Rotations))
.angularVelocity(
velocity.mut_replace(
(motor.getVelocity().getValue() / 2048.0 * 10), RotationsPerSecond)),
this));
}

public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}
Expand Down Expand Up @@ -299,62 +254,6 @@ public Command followTrajectory(String name, boolean isRed) {
this);
}

public Command sysIdQuasistaticDrive(int moduleIndex, SysIdRoutine.Direction direction) {
return m_driveSysIdRoutines[moduleIndex].quasistatic(direction);
}

/**
* Creates a command for a dynamic drive SysId routine for the specified module.
*
* @param moduleIndex The index of the module.
* @param direction The direction of the SysId routine.
* @return A command for the dynamic drive SysId routine.
*/
public Command sysIdDynamicDrive(int moduleIndex, SysIdRoutine.Direction direction) {
return m_driveSysIdRoutines[moduleIndex].dynamic(direction);
}

/**
* Creates a command for a quasistatic steer SysId routine for the specified module.
*
* @param moduleIndex The index of the module.
* @param direction The direction of the SysId routine.
* @return A command for the quasistatic steer SysId routine.
*/
public Command sysIdQuasistaticSteer(int moduleIndex, SysIdRoutine.Direction direction) {
return m_steerSysIdRoutines[moduleIndex].quasistatic(direction);
}

/**
* Creates a command for a dynamic steer SysId routine for the specified module.
*
* @param moduleIndex The index of the module.
* @param direction The direction of the SysId routine.
* @return A command for the dynamic steer SysId routine.
*/
public Command sysIdDynamicSteer(int moduleIndex, SysIdRoutine.Direction direction) {
return m_steerSysIdRoutines[moduleIndex].dynamic(direction);
}

/**
* Gets a list of SysId commands for all modules.
*
* @return A list of SysId commands.
*/
public List<Function<SysIdRoutine.Direction, Command>> getSysIdCommands() {
List<Function<SysIdRoutine.Direction, Command>> sysidCommands = new ArrayList<>();

for (int i = 0; i < 4; i++) {
int moduleIndex = i;
sysidCommands.add(direction -> sysIdDynamicDrive(moduleIndex, direction));
sysidCommands.add(direction -> sysIdQuasistaticDrive(moduleIndex, direction));
sysidCommands.add(direction -> sysIdDynamicSteer(moduleIndex, direction));
sysidCommands.add(direction -> sysIdQuasistaticSteer(moduleIndex, direction));
}

return sysidCommands;
}

public Command getAutoPath(String pathName) {
var initPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathName);
m_odometry.resetPosition(initPose.getRotation(), m_modulePositions, initPose);
Expand Down
33 changes: 0 additions & 33 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@

package frc.robot.subsystems;

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

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
Expand All @@ -15,11 +13,8 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;

public class Intake extends SubsystemBase {
Expand All @@ -35,26 +30,6 @@ public class Intake extends SubsystemBase {
private final PIDController m_pid =
new PIDController(Constants.intakeP, Constants.intakeI, Constants.intakeD);

private final SysIdRoutine m_sysIdRoutine =
new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
m_motor.setVoltage(volts.in(Volts));
},
// log -> {
// log.motor("intake")
// .voltage(
// m_appliedVoltage.mut_replace(
// m_motor.getBusVoltage(), Volts))
// .angularPosition(m_angle.mut_replace(m_encoder.getPosition(), Rotations))
// .angularVelocity(
// m_velocity.mut_replace(m_encoder.getVelocity(), RotationsPerSecond));
// },
null,
this,
"intake"));

public Intake() {}

public Command achieveSpeed(double speed) {
Expand All @@ -80,14 +55,6 @@ public Command intake(double voltage) {
return run(() -> m_motor.setVoltage(voltage));
}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.quasistatic(direction);
}

public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
}

@Override
public void periodic() {
var currentcommand = getCurrentCommand();
Expand Down

0 comments on commit a5cc7cd

Please sign in to comment.