mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-08 16:28:05 -06:00
Fix code
This commit is contained in:
Shikhar
@@ -0,0 +1,79 @@
|
||||
package frc4388.robot.subsystems.climber;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
|
||||
public class Climber extends SubsystemBase {
|
||||
ClimberIO io;
|
||||
ClimberStateAutoLogged state = new ClimberStateAutoLogged();
|
||||
|
||||
Supplier<Pose2d> m_swervePoseSupplier;
|
||||
|
||||
public Climber(
|
||||
ClimberIO io,
|
||||
Supplier<Pose2d> swervePoseSupplier
|
||||
) {
|
||||
this.io = io;
|
||||
this.m_swervePoseSupplier = swervePoseSupplier;
|
||||
}
|
||||
|
||||
// public enum ClimberMode {
|
||||
// Up,
|
||||
// Down,
|
||||
// }
|
||||
|
||||
// public void setMode(IntakeMode mode) {
|
||||
// switch (mode) {
|
||||
// case Up:
|
||||
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
|
||||
// io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
|
||||
// break;
|
||||
// case Down:
|
||||
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
|
||||
// io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
// public enum FieldZone {
|
||||
// // The robot should aim at the hub
|
||||
// InShootZone,
|
||||
// // The robot should aim towards the wall
|
||||
// AimAtWall,
|
||||
|
||||
// }
|
||||
|
||||
// // Calculate what should be done based off of the position of the robot
|
||||
// // TODO: Implement field zones
|
||||
// public FieldZone getTarget(Pose2d position) {
|
||||
// return FieldZone.InShootZone;
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
|
||||
|
||||
// FaultReporter.register(this); // TODO Implement fault reporter
|
||||
|
||||
|
||||
Logger.processInputs("Climber", state);
|
||||
|
||||
Pose2d pose = m_swervePoseSupplier.get();
|
||||
Angle robotRot = pose.getRotation().getMeasure();
|
||||
|
||||
io.updateInputs(state);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
package frc4388.robot.subsystems.climber;
|
||||
|
||||
import static edu.wpi.first.units.Units.Degrees;
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.Slot1Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class ClimberConstants {
|
||||
// Motor conversions
|
||||
|
||||
public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.;
|
||||
|
||||
//IDs
|
||||
|
||||
|
||||
// public static final CanDevice CLIMBER_ID = new CanDevice("SHOOTER 1", 20);
|
||||
|
||||
// Limits
|
||||
|
||||
// 0 is the forward angle on the robot.
|
||||
// negative is left, positive is right
|
||||
// public static final Angle L1 = Degrees.of(-180);
|
||||
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
|
||||
public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0);
|
||||
|
||||
public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0);
|
||||
|
||||
|
||||
public static final Slot0Configs CLIMBER_PID = new Slot0Configs()
|
||||
.withKP(2.0)
|
||||
.withKI(0.0)
|
||||
.withKD(10.0);
|
||||
|
||||
// 0 is paralell to the ground, 90 is directly up
|
||||
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
||||
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
||||
|
||||
// Motor configs
|
||||
public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
}
|
||||
|
||||
// public static final class IDs {
|
||||
// public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
|
||||
// }
|
||||
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
@@ -0,0 +1,43 @@
|
||||
package frc4388.robot.subsystems.climber;
|
||||
|
||||
import static edu.wpi.first.units.Units.Amps;
|
||||
import static edu.wpi.first.units.Units.InchesPerSecond;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.units.CurrentUnit;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import edu.wpi.first.units.measure.LinearVelocity;
|
||||
|
||||
public interface ClimberIO {
|
||||
@AutoLog
|
||||
public class ClimberState {
|
||||
Distance climberDistance = Inches.of(0);
|
||||
Distance climberTargetDistance = Inches.of(0);
|
||||
Current climberMotorCurrent = Amps.of(0);
|
||||
|
||||
// Distance shooterPitch = Rotations.of(0);
|
||||
// Angle shooterTargetPitch = Rotations.of(0);
|
||||
// Current pitchMotorCurrent = Amps.of(0);
|
||||
|
||||
// AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
|
||||
// AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
|
||||
// Current rollerMotorCurrent = Amps.of(0);
|
||||
|
||||
// LinearVelocity feederVelocity = InchesPerSecond.of(0);
|
||||
// LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
|
||||
// Current feederMotorCurrent = Amps.of(0);
|
||||
}
|
||||
|
||||
// public default void setShooterAngle(ShooterState state, Angle angle) {}
|
||||
// public default void setShooterPitch(ShooterState state, Angle angle) {}
|
||||
public default void setClimberDistance(ClimberState state, Distance distance) {}
|
||||
|
||||
public default void updateInputs(ClimberState state) {}
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
package frc4388.robot.subsystems.climber;
|
||||
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.InchesPerSecond;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.*;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.intake.IntakeConstants;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class ClimberReal implements ClimberIO {
|
||||
|
||||
|
||||
TalonFX m_climberMotor;
|
||||
|
||||
public ClimberReal(
|
||||
|
||||
TalonFX climberMotor
|
||||
) {
|
||||
// m_angleMotor = angleMotor;
|
||||
// m_pitchMotor = pitchMotor;
|
||||
m_climberMotor = climberMotor;
|
||||
// Apply the configs
|
||||
m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID);
|
||||
m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG);
|
||||
|
||||
}
|
||||
|
||||
private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){
|
||||
if(distance.gt(climberLimitUpper)) {
|
||||
return climberLimitUpper;
|
||||
}else if(distance.lt(climberLimitLower)) {
|
||||
return climberLimitLower;
|
||||
}else{
|
||||
return distance;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void setClimberDistance(ClimberState state, Distance distance) {
|
||||
state.climberTargetDistance = distance;
|
||||
// Assume that the angle is always accurate, because I think we will use a shaft encoder
|
||||
// Assume that 0 degrees = forwards. Might need an offset here
|
||||
|
||||
Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER);
|
||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||
double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
|
||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance);
|
||||
m_climberMotor.setControl(posRequest);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(ClimberState state) {
|
||||
double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations);
|
||||
double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
|
||||
state.climberDistance = Inches.of(linearInches);
|
||||
state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue();
|
||||
|
||||
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
|
||||
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
|
||||
|
||||
// state.armAngle = m_armMotor.getPosition().getValue();
|
||||
// state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user