-
Notifications
You must be signed in to change notification settings - Fork 0
Azoulay #9
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
Azoulay #9
Changes from all commits
5e0694a
393d100
5fbe4a6
fba1a7f
da85522
35f9790
9eebd78
8bb0443
dfb0223
9d1f535
c36928f
6ae9dc7
e46b8e1
8b2c101
f427577
1835e45
86e06bb
c39f5d4
65b6243
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,67 @@ | ||
package frc.trigon.robot.subsystems.sideshooter; | ||
|
||
|
||
import com.ctre.phoenix6.hardware.TalonFX; | ||
import com.revrobotics.CANSparkMax; | ||
import edu.wpi.first.math.geometry.Rotation2d; | ||
import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
import edu.wpi.first.math.util.Units; | ||
import edu.wpi.first.wpilibj.Timer; | ||
import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
import frc.trigon.robot.utilities.Conversions; | ||
|
||
public class SideShooter extends SubsystemBase { | ||
private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; | ||
private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; | ||
private TrapezoidProfile angleMotorProfile = null; | ||
private double lastAngleMotorProfileGenerationTime; | ||
|
||
private final static SideShooter INSTANCE = new SideShooter(); | ||
|
||
public static SideShooter getInstance() { | ||
return INSTANCE; | ||
} | ||
|
||
azdaniel123 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
double getAnglePosition() { | ||
double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); | ||
return positionRevolutions; | ||
} | ||
Comment on lines
+25
to
+28
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. return a rotation2d here. |
||
|
||
Double getAngleVelocityDegreesPerSecond() { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. use the primative type double. |
||
return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); | ||
} | ||
|
||
double getAngleMotorProfileTime() { | ||
return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; | ||
} | ||
Comment on lines
+25
to
+36
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. move these to the bottom of the class and make them private. |
||
|
||
void generateAngleMotorProfile(Rotation2d targetAngle) { | ||
angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, | ||
new TrapezoidProfile.State(targetAngle.getDegrees(), 0), | ||
new TrapezoidProfile.State(getAnglePosition(), getAngleVelocityDegreesPerSecond())); | ||
|
||
lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); | ||
} | ||
|
||
double calculateAngleOutput(TrapezoidProfile.State targetState) { | ||
double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( | ||
getAnglePosition(), | ||
targetState.position | ||
); | ||
double feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( | ||
Units.degreesToRadians(targetState.position), | ||
targetState.velocity | ||
); | ||
angleMotor.setVoltage(pidOutPut = feedforward); | ||
Comment on lines
+54
to
+55
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. why is this here? |
||
return feedforward + pidOutPut; | ||
} | ||
Comment on lines
+46
to
+57
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. move this down and make it private. |
||
|
||
void setTargetAngleMotorProfileTime() { | ||
if (angleMotorProfile == null) { | ||
angleMotor.stopMotor(); | ||
return; | ||
} | ||
TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); | ||
angleMotor.setVoltage(calculateAngleOutput(targetState)); | ||
} | ||
} | ||
azdaniel123 marked this conversation as resolved.
Show resolved
Hide resolved
|
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
package frc.trigon.robot.subsystems.sideshooter; | ||
|
||
import edu.wpi.first.math.geometry.Rotation2d; | ||
import edu.wpi.first.wpilibj2.command.Command; | ||
import edu.wpi.first.wpilibj2.command.FunctionalCommand; | ||
|
||
public class SideShooterCommands { | ||
|
||
public static SideShooterCommands getInstance() { | ||
return getInstance(); | ||
} | ||
|
||
Comment on lines
+9
to
+12
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Huh? |
||
|
||
public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { | ||
return new FunctionalCommand( | ||
() -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), | ||
() -> getSetTargetShooterAngleCommand(targetAngle), | ||
(interrupted) -> {}, | ||
() -> false, | ||
SideShooter.getInstance() | ||
); | ||
} | ||
Comment on lines
+14
to
+22
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Make this static, and instead of typing |
||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,107 @@ | ||
package frc.trigon.robot.subsystems.sideshooter; | ||
|
||
import com.ctre.phoenix6.StatusSignal; | ||
import com.ctre.phoenix6.configs.CANcoderConfiguration; | ||
import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
import com.ctre.phoenix6.hardware.CANcoder; | ||
import com.ctre.phoenix6.hardware.TalonFX; | ||
import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; | ||
import com.ctre.phoenix6.signals.InvertedValue; | ||
import com.ctre.phoenix6.signals.NeutralModeValue; | ||
import com.ctre.phoenix6.signals.SensorDirectionValue; | ||
import com.revrobotics.CANSparkMax; | ||
import com.revrobotics.CANSparkMaxLowLevel; | ||
import edu.wpi.first.math.controller.ArmFeedforward; | ||
import edu.wpi.first.math.controller.PIDController; | ||
import edu.wpi.first.math.geometry.Rotation2d; | ||
import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
|
||
public class SideShooterConstants { | ||
azdaniel123 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 0, ENCODER_ID = 0; | ||
private static final double ANGLE_ENCODER_OFFSET = 0; | ||
private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; | ||
private static final AbsoluteSensorRangeValue ANGLE_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; | ||
private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; | ||
private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; | ||
private static final boolean ANGLE_MOTOR_INVERTED = false; | ||
private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; | ||
private static final double VOLTAGE_COMPENSATION_SATURATION = 12; | ||
static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); | ||
static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); | ||
private static final CANcoder ANGLE_ENCODER = new CANcoder(ENCODER_ID); | ||
|
||
private static final double | ||
MAX_ANGLE_VELOCITY = 600, | ||
MAX_ANGLE_ACCELERATION = 500; | ||
static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( | ||
MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION | ||
); | ||
|
||
private static final double | ||
ANGLE_MOTOR_P = 0, | ||
ANGLE_MOTOR_I = 0, | ||
ANGLE_MOTOR_D = 0; | ||
static final PIDController ANGLE_PID_CONTROLLER = new PIDController( | ||
ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D | ||
); | ||
|
||
private static final double | ||
ANGLE_MOTOR_KS = 0.58835, | ||
ANGLE_MOTOR_KV = 0.74627, | ||
ANGLE_MOTOR_KA = 0.37502, | ||
ANGLE_MOTOR_KG = 0.92056; | ||
static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( | ||
ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA | ||
); | ||
|
||
static final StatusSignal<Double> ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); | ||
|
||
static { | ||
configureAngleEncoder(); | ||
configureAngleMotor(); | ||
configureShootingMotor(); | ||
} | ||
|
||
private static void configureShootingMotor() { | ||
TalonFXConfiguration config = new TalonFXConfiguration(); | ||
config.Audio.BeepOnBoot = false; | ||
config.Audio.BeepOnConfig = false; | ||
config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; | ||
config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; | ||
SHOOTING_MOTOR.getConfigurator().apply(config); | ||
azdaniel123 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
SHOOTING_MOTOR.optimizeBusUtilization(); | ||
} | ||
|
||
private static void configureAngleMotor() { | ||
ANGLE_MOTOR.restoreFactoryDefaults(); | ||
ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); | ||
ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); | ||
ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); | ||
} | ||
|
||
private static void configureAngleEncoder() { | ||
CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); | ||
configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_VALUE; | ||
configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; | ||
configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; | ||
ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); | ||
azdaniel123 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
||
ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); | ||
ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); | ||
ANGLE_ENCODER.optimizeBusUtilization(); | ||
} | ||
|
||
public enum SideShooterState { | ||
COLLECT_POSITION(Rotation2d.fromDegrees(0), 0), | ||
MID_LEVEL_POSITION(Rotation2d.fromDegrees(222), 9), | ||
HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3), 78); | ||
|
||
final Rotation2d angle; | ||
final double shootingVoltage; | ||
|
||
SideShooterState(Rotation2d angle, double shootingVoltage) { | ||
this.angle = angle; | ||
this.shootingVoltage = shootingVoltage; | ||
} | ||
} | ||
} |
Uh oh!
There was an error while loading. Please reload this page.