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

Commit

Permalink
arm
Browse files Browse the repository at this point in the history
  • Loading branch information
@Superbro525Alt
Superbro525Alt committed Aug 8, 2024
1 parent 0a68caf commit d8a88ec
Show file tree
Hide file tree
Showing 7 changed files with 358 additions and 0 deletions.
17 changes: 17 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@
import frc.robot.subsystems.Index;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Sysid;

import java.util.HashMap;

public class Robot extends CommandRobot {
Expand All @@ -33,6 +35,7 @@ public class Robot extends CommandRobot {
private final Climber m_climber = new Climber();
private final Intake m_intake = new Intake();
private final Index m_index = new Index();
private final Sysid m_sysid = new Sysid();
private final Superstructure m_superstructure = new Superstructure(m_shooter, m_intake, m_index);
private static final CommandSwerveDrivetrain m_drivetrain = TunerConstants.DriveTrain;

Expand All @@ -58,6 +61,20 @@ public Robot() {
DriverStation.startDataLog(DataLogManager.getLog());
SignalLogger.setPath(DataLogManager.getLogDir());
SignalLogger.start();
m_sysid.add(m_shooter::sysIdDynamic);
m_sysid.add(m_shooter::sysIdQuasistatic);

m_sysid.add(m_arm::sysIdDynamic);
m_sysid.add(m_arm::sysIdQuasistatic);

m_sysid.add(m_climber::sysIdDynamic);
m_sysid.add(m_climber::sysIdQuasistatic);

m_sysid.add(m_intake::sysIdDynamic);
m_sysid.add(m_intake::sysIdQuasistatic);

m_sysid.addAll(m_drivetrain.getSysIdCommands());

m_drivetrain.registerTelemetry(m_logger::telemeterize);

m_scheduler.registerSubsystem(m_arm);
Expand Down
49 changes: 49 additions & 0 deletions src/main/java/frc/robot/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,21 +4,34 @@

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 @@ -61,6 +74,34 @@ public enum Setpoint {
private final DoublePublisher angle = driveStats.getDoubleTopic("Angle").publish();
public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);

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(5)
),
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 @@ -150,4 +191,12 @@ public Command goToSetpoint(Setpoint setpoint) {
public void periodic() {
angle.set(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: 42 additions & 0 deletions src/main/java/frc/robot/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,27 @@

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 @@ -26,6 +38,28 @@ 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 @@ -43,4 +77,12 @@ 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);
}
}
102 changes: 102 additions & 0 deletions src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,26 +4,41 @@

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.Orchestra;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.AudioConfigs;
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;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
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.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.function.Function;
import java.util.function.Supplier;

/**
Expand Down Expand Up @@ -53,6 +68,9 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
/* 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 @@ -83,6 +101,33 @@ 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());
}
}

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) {
Expand Down Expand Up @@ -203,4 +248,61 @@ public Command followTrajectory(String name, boolean isRed) {
() -> 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;
}

}
Loading

0 comments on commit d8a88ec

Please sign in to comment.