Skip to content

Commit

Permalink
added sysid for all subsystem I can
Browse files Browse the repository at this point in the history
  • Loading branch information
@finnsherman
finnsherman committed Feb 2, 2024
1 parent c13d562 commit c61939a
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 56 deletions.
95 changes: 49 additions & 46 deletions src/main/java/frc/robot/subsystems/IndexerSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless;
import static frc.robot.Constants.IndexerConstants.DEVICE_ID_INDEXER;
import static frc.robot.Constants.IndexerConstants.THRESHOLD_INTAKE;
import static frc.robot.Constants.IndexerConstants.THRESHOLD_SPACE;
import static frc.robot.Constants.IndexerConstants.THRESHOLD_SHOOTER;

import java.util.Map;

Expand All @@ -24,27 +24,27 @@
public class IndexerSubsystem extends SubsystemBase {
public enum IndexerState {
noRing,
intakingRing,
intakingRing,
HasRing,
processingRing
}

public IndexerState indexerState;
public IndexerState indexerState;

public boolean capturedRing() {
return (this.indexerState != IndexerState.noRing);
}

public IndexerState getIndexerState() {
return indexerState;
return indexerState;
}

private boolean isIntakeSensorTripped() {
return colorSensorReader.getIntakeValues().proximity > THRESHOLD_INTAKE;
}

public boolean isShooterSensorTripped() {
return colorSensorReader.getSpacerValues().proximity > THRESHOLD_SPACE;
return colorSensorReader.getShooterValues().proximity > THRESHOLD_SHOOTER;
}

public void stop() {
Expand All @@ -59,63 +59,66 @@ public void load() {
private SparkPIDController pidController;
private final ColorSensorReader colorSensorReader = new ColorSensorReader();
private final Notifier colorSensorNotifier = new Notifier(colorSensorReader);

public IndexerSubsystem() {
indexer.restoreFactoryDefaults();
indexer.enableVoltageCompensation(12);
indexer.setOpenLoopRampRate(0.1);
indexer.setClosedLoopRampRate(0.1);
indexer.setInverted(true);
pidController = indexer.getPIDController();
pidController.setP(IndexerConstants.BELT_kP);
pidController.setFF(0.00009);
indexer.getEncoder();
indexer.burnFlash();
indexer.setIdleMode(IdleMode.kCoast);

colorSensorReader.run();
// Update the color sensors in the background to prevent loop overrun
colorSensorNotifier.setName("Color Sensors");
colorSensorNotifier.startPeriodic(0.02);
indexer.restoreFactoryDefaults();
indexer.enableVoltageCompensation(12);
indexer.setOpenLoopRampRate(0.1);
indexer.setClosedLoopRampRate(0.1);
indexer.setInverted(true);
pidController = indexer.getPIDController();
pidController.setP(IndexerConstants.BELT_kP);
pidController.setFF(0.00009);
indexer.getEncoder();
indexer.burnFlash();
indexer.setIdleMode(IdleMode.kCoast);

colorSensorReader.run();
// Update the color sensors in the background to prevent loop overrun
colorSensorNotifier.setName("Color Sensors");
colorSensorNotifier.startPeriodic(0.02);

}

public void addDashboardWidgets(ShuffleboardLayout dashboard) {
var detailDashboard = dashboard.getLayout("Detail", BuiltInLayouts.kGrid)
.withProperties(Map.of("Number of columns", 2, "Number of rows", 3)).withPosition(0, 0);
detailDashboard.addNumber("Intake Prox", () -> colorSensorReader.getIntakeValues().proximity).withPosition(0, 0);
detailDashboard.addNumber("Shooter Prox", () -> colorSensorReader.getIntakeValues().proximity).withPosition(0, 0);
detailDashboard.addString("Ring Type", () -> this.getIndexerState().name()).withPosition(1, 1);

}

public void addDashboardWidgets(ShuffleboardLayout dashboard) {
var detailDashboard = dashboard.getLayout("Detail", BuiltInLayouts.kGrid)
.withProperties(Map.of("Number of columns", 2, "Number of rows", 3)).withPosition(0, 0);
detailDashboard.addNumber("Intake Prox", () -> colorSensorReader.getIntakeValues().proximity).withPosition(0, 0);
detailDashboard.addNumber("Shooter Prox", () -> colorSensorReader.getIntakeValues().proximity).withPosition(0, 0);
detailDashboard.addString("Ring Type", () -> this.getIndexerState().name() ).withPosition(1, 1);

}

public void addDriverDashboardWidgets(ShuffleboardTab dashboard) {
dashboard.addBoolean("Ring Position", this::capturedRing).withWidget(BuiltInWidgets.kDial)
dashboard.addBoolean ("Ring Position", this::capturedRing).withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of("Min", 0, "Max", 2)).withSize(1, 1).withPosition(8, 3);
var colorSensorLayout = dashboard.getLayout("Indexer", BuiltInLayouts.kGrid)
.withProperties(Map.of("Number of columns", 1, "Number of rows", 3))
.withSize(1, 3).withPosition(8, 0);
colorSensorLayout.addBoolean("Spacer", () -> true).withPosition(0, 1);
colorSensorLayout.addBoolean("Intake", () -> true).withPosition(0, 2);
colorSensorLayout.addBoolean("Shooter", () -> true).withPosition(0, 1);
colorSensorLayout.addBoolean("Intake", () -> true).withPosition(0, 2);


}

public void intake() {
// sensors return false when something is detected
if ((!isIntakeSensorTripped() && !isShooterSensorTripped()) || // if all sensors are clear stop the belt
(isShooterSensorTripped())) { // if fullsensor is tripped OR intake and 2nd sensor are clear
stop();
} else {
load();
public void intake() {
// sensors return false when something is detected
if ((!isIntakeSensorTripped() && !isShooterSensorTripped()) || //if all sensors are clear stop the belt
(isShooterSensorTripped())) { //if fullsensor is tripped OR intake and 2nd sensor are clear
stop();
} else {
load();
}
}
}

public void prepareToShoot() {
if (isShooterSensorTripped()) {
stop();
} else {
load();
public void prepareToShoot() {
if (isShooterSensorTripped()) {
stop();
} else {
load();
}
}
}

}
41 changes: 31 additions & 10 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,28 +6,39 @@

package frc.robot.subsystems;

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

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.subsystems.sysid.SysIdRoutineSignalLogger;



public class IntakeSubsystem extends SubsystemBase {
/**
*
*/
public IntakeSubsystem() {
var shooterMotorConfig = new TalonFXConfiguration();
shooterMotorConfig.Slot0.kP = 5; // An error of 1 rotation per second results in 5 amps output
shooterMotorConfig.Slot0.kI = 0.1; // An error of 1 rotation per second increases output by 0.1 amps every second
shooterMotorConfig.Slot0.kD = 0.001; // A change of 1000 rotation per second squared results in 1 amp output
}


private VoltageOut voltageRequest = new VoltageOut(0);
private final TalonFX IntakeMotor = new TalonFX(1);

private SysIdRoutine intakeMotorSysIdRoutine = new SysIdRoutine(
new SysIdRoutine.Config(null, null, null, SysIdRoutineSignalLogger.logState()),
new SysIdRoutine.Mechanism((volts) ->
IntakeMotor.setControl(voltageRequest.withOutput(volts.in(Volts))), null, this));

public IntakeSubsystem() {
var intakeMotorConfig = new TalonFXConfiguration();
intakeMotorConfig.Slot0.kP = 5; // An error of 1 rotation per second results in 5 amps output
intakeMotorConfig.Slot0.kI = 0.1; // An error of 1 rotation per second increases output by 0.1 amps every second
intakeMotorConfig.Slot0.kD = 0.001; // A change of 1000 rotation per second squared results in 1 amp output
IntakeMotor.getConfigurator().apply(intakeMotorConfig);
}

private final VelocityTorqueCurrentFOC IntakerMotorVelocity = new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false,
false);

Expand Down Expand Up @@ -57,6 +68,16 @@ public Command Spit() {
IntakeMotor.setControl(IntakerMotorVelocity.withVelocity(-1));
});
}
public Command sysIdIntakeMotorQuasiCommand(Direction direction) {
return intakeMotorSysIdRoutine.quasistatic(direction).withName("SysId Drive Quasistatic " + direction)
.finallyDo(this::Stop);
}

public Command sysIdIntakeMotorDynamCommand(Direction direction) {
return intakeMotorSysIdRoutine.dynamic(direction).withName("SysId Drive Quasistatic " + direction)
.finallyDo(this::Stop);
}


public Command Stop() {
return runOnce(
Expand Down
26 changes: 26 additions & 0 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.subsystems;

import static com.revrobotics.SparkAbsoluteEncoder.Type.kDutyCycle;
import static edu.wpi.first.units.Units.Volts;
import static frc.robot.Constants.ShooterConstants.ACTUATOR_CANCORDER;
import static frc.robot.Constants.ShooterConstants.SHOOTER_DONUT_POSITION_CONTROl;
import static frc.robot.Constants.ShooterConstants.SHOOTER_VELOCITY_CONTROl;
Expand All @@ -9,6 +10,7 @@
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
Expand All @@ -21,7 +23,11 @@
import com.revrobotics.SparkPIDController;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.subsystems.sysid.SysIdRoutineSignalLogger;

public class ShooterSubsystem extends SubsystemBase {
private final TalonFX shooterLeftMotor = new TalonFX(SHOOTER_DONUT_POSITION_CONTROl);
Expand All @@ -38,6 +44,16 @@ public class ShooterSubsystem extends SubsystemBase {

// Offset in rotations to add to encoder value - offset from arm horizontal to
// sensor zero

private VoltageOut voltageRequest = new VoltageOut(0);

private SysIdRoutine ShooterMotorSysIdRoutine = new SysIdRoutine(
new SysIdRoutine.Config(null, null, null, SysIdRoutineSignalLogger.logState()),
new SysIdRoutine.Mechanism((volts) -> {
shooterRightMotor.setControl(voltageRequest.withOutput(volts.in(Volts)));
shooterLeftMotor.setControl(voltageRequest.withOutput(volts.in(Volts)));
}, null, this));

private static final double ENCODER_OFFSET = -0.2285f;

private final VelocityTorqueCurrentFOC shooterMotorVelocity = new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false,
Expand Down Expand Up @@ -111,6 +127,16 @@ public void spinShooterWheel(int x) {
shooterRightMotor.setControl(shooterMotorVelocity.withVelocity(x));
}

public Command sysIdShooterMotorQuasiCommand(Direction direction) {
return ShooterMotorSysIdRoutine.quasistatic(direction).withName("SysId Drive Quasistatic " + direction)
.finallyDo(this::stop);
}

public Command sysIdShooterMotorDynamCommand(Direction direction) {
return ShooterMotorSysIdRoutine.dynamic(direction).withName("SysId Drive Quasistatic " + direction)
.finallyDo(this::stop);
}

public void activeStop() {
if (!isActiveStopped) {
shooterLeftMotor.setControl(actuatorMotorVelocity.withVelocity(0));
Expand Down
19 changes: 19 additions & 0 deletions src/main/java/frc/robot/subsystems/sysid/SysIdRoutineSignalLogger.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package frc.robot.subsystems.sysid;

import java.util.function.Consumer;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.hardware.ParentDevice;

import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;

public class SysIdRoutineSignalLogger extends SignalLogger {
public static Consumer<SysIdRoutineLog.State> logState() {
start(); // Start logging if we get the consumer, so we have some data before the start of the motion
return (SysIdRoutineLog.State state) -> writeString("State", state.toString());
}

public static void registerAsSysIdLog(ParentDevice device) {
writeInteger("SysId Logged Device", device.getDeviceHash(), "");
}
}

0 comments on commit c61939a

Please sign in to comment.