mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Sparkmax for arm motor
This commit is contained in:
Shatcar
@@ -96,19 +96,19 @@ public class Intake extends SubsystemBase {
|
||||
|
||||
switch (mode) {
|
||||
case Extended:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
|
||||
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
|
||||
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
|
||||
// io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
|
||||
break;
|
||||
case Retracted:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
|
||||
io.setRollerOutput(state, 0);
|
||||
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
|
||||
// io.setRollerOutput(state, 0);
|
||||
break;
|
||||
case Extending:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()));
|
||||
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
|
||||
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
|
||||
break;
|
||||
case Retracting:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()));
|
||||
io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
|
||||
io.setRollerOutput(state, 0);
|
||||
break;
|
||||
case Bouncing:
|
||||
|
||||
@@ -1,16 +1,10 @@
|
||||
package frc4388.robot.subsystems.intake;
|
||||
|
||||
import static edu.wpi.first.units.Units.Amps;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import com.revrobotics.spark.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
|
||||
import com.revrobotics.spark.config.LimitSwitchConfig.Type;
|
||||
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
@@ -33,7 +27,7 @@ public class IntakeConstants {
|
||||
|
||||
public static final CanDevice ARM_ID = new CanDevice("ARM", 20);
|
||||
public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21);
|
||||
public static final int ARM_LIMIT_SWITCH_CHANNEL = 9;
|
||||
// public static final int ARM_LIMIT_SWITCH_CHANNEL = 9;
|
||||
|
||||
// Limits
|
||||
|
||||
@@ -46,7 +40,7 @@ public class IntakeConstants {
|
||||
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
|
||||
|
||||
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1);
|
||||
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33);
|
||||
public static final ConfigurableDouble ARM_ssLIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33);
|
||||
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4);
|
||||
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4);
|
||||
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70);
|
||||
@@ -56,16 +50,16 @@ public class IntakeConstants {
|
||||
// public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
|
||||
|
||||
|
||||
public static final Slot0Configs ARM_PID = new Slot0Configs()
|
||||
.withKP(0.08)
|
||||
.withKI(0.0)
|
||||
.withKD(0.05);
|
||||
// public static final Slot0Configs ARM_PID = new Slot0Configs()
|
||||
// .withKP(0.08)
|
||||
// .withKI(0.0)
|
||||
// .withKD(0.05);
|
||||
|
||||
|
||||
|
||||
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.08);
|
||||
public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
|
||||
public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
|
||||
// public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.08);
|
||||
// public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
|
||||
// public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
|
||||
|
||||
|
||||
|
||||
@@ -75,25 +69,37 @@ public class IntakeConstants {
|
||||
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
||||
|
||||
// Motor configs
|
||||
public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(15) // 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 TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
public static final SparkMaxConfig ARM_MOTOR_CONFIG = new SparkMaxConfig();
|
||||
public static final SparkMaxConfig ROLELR_MOTOR_CONFIG = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
ARM_MOTOR_CONFIG.limitSwitch
|
||||
.reverseLimitSwitchType(Type.kNormallyClosed)
|
||||
.limitSwitchPositionSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.forwardLimitSwitchPosition(0)
|
||||
.forwardLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition);
|
||||
}
|
||||
|
||||
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(15) // 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 TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true)
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
}
|
||||
|
||||
@@ -23,7 +23,7 @@ public interface IntakeIO {
|
||||
Current armMotorCurrent = Amps.of(0);
|
||||
|
||||
AngularVelocity armMotorVelocity = RotationsPerSecond.of(0);
|
||||
AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0);
|
||||
// AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0);
|
||||
|
||||
// Angle shooterPitch = Rotations.of(0);
|
||||
// Angle shooterTargetPitch = Rotations.of(0);
|
||||
|
||||
@@ -1,7 +1,10 @@
|
||||
package frc4388.robot.subsystems.intake;
|
||||
|
||||
import static edu.wpi.first.units.Units.Amps;
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
@@ -15,44 +18,24 @@ import edu.wpi.first.units.measure.Acceleration;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Velocity;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.DutyCycleEncoder;
|
||||
|
||||
public class IntakeReal implements IntakeIO {
|
||||
|
||||
TalonFX m_armMotor;
|
||||
SparkMax m_armMotor;
|
||||
SparkMax m_rollerMotor;
|
||||
DigitalInput m_armLimitSwitch;
|
||||
|
||||
PositionDutyCycle armPosition = new PositionDutyCycle(0);
|
||||
DutyCycleOut armPercentOutput = new DutyCycleOut(0);
|
||||
DutyCycleEncoder encoder;
|
||||
|
||||
public IntakeReal(
|
||||
DigitalInput armLimitSwitch,
|
||||
TalonFX armMotor,
|
||||
// DigitalInput armLimitSwitch,
|
||||
SparkMax armMotor,
|
||||
SparkMax rollerMotor
|
||||
) {
|
||||
// m_angleMotor = angleMotor;
|
||||
// m_pitchMotor = pitchMotor;
|
||||
m_armMotor = armMotor;
|
||||
m_rollerMotor = rollerMotor;
|
||||
m_armLimitSwitch = armLimitSwitch;
|
||||
|
||||
// Apply the configs
|
||||
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
|
||||
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
|
||||
// m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
|
||||
|
||||
armPosition.Slot = 0;
|
||||
// rollerVelocity.Slot = 0;
|
||||
}
|
||||
|
||||
private Angle clampAng(Angle x, Angle min, Angle max){
|
||||
if(x.gt(max)) {
|
||||
return max;
|
||||
}else if(x.lt(min)) {
|
||||
return min;
|
||||
}else{
|
||||
return x;
|
||||
}
|
||||
// m_armLimitSwitch = armLimitSwitch;
|
||||
}
|
||||
|
||||
|
||||
@@ -77,40 +60,14 @@ public class IntakeReal implements IntakeIO {
|
||||
Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
|
||||
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||
m_armMotor.setControl(
|
||||
armPosition
|
||||
.withPosition(motorAngle)
|
||||
.withLimitReverseMotion(!m_armLimitSwitch.get())
|
||||
);
|
||||
// m_armMotor.setControl(
|
||||
// armPosition
|
||||
// .withPosition(motorAngle)
|
||||
// .withLimitReverseMotion(!m_armLimitSwitch.get())
|
||||
// );
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testSetArmAngle(IntakeState state, Angle angle){
|
||||
state.armTargetAngle = angle;
|
||||
Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
|
||||
final TrapezoidProfile m_profile = new TrapezoidProfile(
|
||||
new TrapezoidProfile.Constraints(80, 160)
|
||||
);
|
||||
|
||||
// Final target of motorAngle rot, 0 rps
|
||||
// Convert the Angle to a numeric degree value before creating the profile state
|
||||
TrapezoidProfile.State m_goal = new TrapezoidProfile.State(motorAngle.in(Rotations), 0);
|
||||
TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
|
||||
|
||||
// create a position closed-loop request, voltage output, slot 0 configs
|
||||
final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
|
||||
|
||||
// calculate the next profile setpoint
|
||||
m_setpoint = m_profile.calculate(0.020, m_setpoint, m_goal);
|
||||
|
||||
// send the request to the device
|
||||
m_request.Position = m_setpoint.position;
|
||||
m_request.Velocity = m_setpoint.velocity;
|
||||
m_armMotor.setControl(m_request);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stopArm(){
|
||||
m_armMotor.set(0);
|
||||
@@ -119,36 +76,33 @@ public class IntakeReal implements IntakeIO {
|
||||
|
||||
@Override
|
||||
public void armOutput(double percentOutput){
|
||||
m_armMotor.setControl(
|
||||
armPercentOutput.withOutput(percentOutput)
|
||||
.withLimitReverseMotion(!m_armLimitSwitch.get())
|
||||
);
|
||||
m_armMotor.set(percentOutput);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(IntakeState state) {
|
||||
state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
|
||||
state.armAngle = Rotations.of(m_armMotor.getEncoder().getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
state.armMotorVelocity = RotationsPerSecond.of(m_armMotor.getEncoder().getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
// state.armMotorAcceleration = RotationsPerSecondPerSecond.of(m_armMotor.getEncoder().ge);
|
||||
state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent());
|
||||
|
||||
state.rollerOutput = m_rollerMotor.get();
|
||||
state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent());
|
||||
state.retractedLimit = !m_armLimitSwitch.get();
|
||||
|
||||
state.armMotorVelocity = m_armMotor.getVelocity().getValue();
|
||||
state.armMotorAcceleration = m_armMotor.getAcceleration().getValue();
|
||||
|
||||
if(state.retractedLimit) {
|
||||
// Set the arm motor to be zero if the limit switch is pressed
|
||||
m_armMotor.setPosition(0., 0);
|
||||
}
|
||||
|
||||
// if(state.retractedLimit) {
|
||||
// // Set the arm motor to be zero if the limit switch is pressed
|
||||
// m_armMotor.setPosition(0., 0);
|
||||
// }
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateGains() {
|
||||
|
||||
IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
|
||||
IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
|
||||
IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
|
||||
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
|
||||
// IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
|
||||
// IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
|
||||
// IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
|
||||
// m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user