From d46d959819df7cc2bb2ee3b0c4a131f79191a35a Mon Sep 17 00:00:00 2001 From: yisha23 Date: Mon, 20 Nov 2023 17:52:58 +0200 Subject: [PATCH 01/18] added sideShooterConstantas --- .../sideshooter/SideShooterConstants.java | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java new file mode 100644 index 000000000..15cf375ad --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -0,0 +1,54 @@ +package frc.trigon.robot.subsystems.sideshooter; + +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.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; + +public class SideShooterConstants { + private static final int SHOOTING_MOTOR_ID = 0; + private static final int ANGEL_MOTOR_ID = 0; + private static final int ENCODER_ID = 0; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final CANSparkMax.IdleMode ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + private static final InvertedValue SHOOTING_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_INVERTED_VALUE = false; + private static final double ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + static final CANcoder ENCODER = new CANcoder(ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGEL_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + + static { + configureShootingMotor(); + configureAngleMotor(); + configureEncoder(); + } + private static void configureShootingMotor() { + TalonFXConfiguration ShootingMotorConfig = new TalonFXConfiguration(); + ShootingMotorConfig.Audio.BeepOnBoot = false; + ShootingMotorConfig.Audio.BeepOnConfig = false; + ShootingMotorConfig.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; + ShootingMotorConfig.MotorOutput.Inverted = SHOOTING_INVERTED_VALUE; + SHOOTING_MOTOR.getConfigurator().apply(ShootingMotorConfig); + } + + private static void configureAngleMotor(){ + ANGEL_MOTOR.restoreFactoryDefaults(); + ANGEL_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); + ANGEL_MOTOR.setInverted(ANGLE_INVERTED_VALUE); + } + + private static void configureEncoder(){ + CANcoderConfiguration EncoderConfiguration = new CANcoderConfiguration(); + EncoderConfiguration.MagnetSensor.MagnetOffset = ENCODER_OFFSET; + EncoderConfiguration.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; + ENCODER.getConfigurator().apply(EncoderConfiguration); + } + +} From 930fe123bd8cb65abf538223fe2f4d27fb9ed30c Mon Sep 17 00:00:00 2001 From: yisha23 Date: Wed, 22 Nov 2023 18:02:30 +0200 Subject: [PATCH 02/18] fix according to the rewiew --- .../sideshooter/SideShooterConstants.java | 48 ++++++++++--------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 15cf375ad..2d51c076c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -11,44 +11,48 @@ import com.revrobotics.CANSparkMaxLowLevel; public class SideShooterConstants { - private static final int SHOOTING_MOTOR_ID = 0; - private static final int ANGEL_MOTOR_ID = 0; - private static final int ENCODER_ID = 0; + private static final int + SHOOTING_MOTOR_ID = 0, + ANGLE_MOTOR_ID = 0, + ANGLE_ENCODER_ID = 0; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; - private static final CANSparkMax.IdleMode ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final InvertedValue SHOOTING_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - private static final boolean ANGLE_INVERTED_VALUE = false; + private static final boolean ANGLE_INVERTED = false; private static final double ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - static final CANcoder ENCODER = new CANcoder(ENCODER_ID); + private static final double VOLTAGE_COMPENSATION_VALUE = 0; + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); - static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGEL_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static { configureShootingMotor(); configureAngleMotor(); - configureEncoder(); + configureAngleEncoder(); } + private static void configureShootingMotor() { - TalonFXConfiguration ShootingMotorConfig = new TalonFXConfiguration(); - ShootingMotorConfig.Audio.BeepOnBoot = false; - ShootingMotorConfig.Audio.BeepOnConfig = false; - ShootingMotorConfig.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; - ShootingMotorConfig.MotorOutput.Inverted = SHOOTING_INVERTED_VALUE; - SHOOTING_MOTOR.getConfigurator().apply(ShootingMotorConfig); + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; + config.MotorOutput.Inverted = SHOOTING_INVERTED_VALUE; + SHOOTING_MOTOR.getConfigurator().apply(config); } private static void configureAngleMotor(){ ANGEL_MOTOR.restoreFactoryDefaults(); - ANGEL_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); - ANGEL_MOTOR.setInverted(ANGLE_INVERTED_VALUE); + ANGEL_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + ANGEL_MOTOR.setInverted(ANGLE_INVERTED); + ANGEL_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_VALUE); } - private static void configureEncoder(){ - CANcoderConfiguration EncoderConfiguration = new CANcoderConfiguration(); - EncoderConfiguration.MagnetSensor.MagnetOffset = ENCODER_OFFSET; - EncoderConfiguration.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; - ENCODER.getConfigurator().apply(EncoderConfiguration); + private static void configureAngleEncoder(){ + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; + config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; + ANGLE_ENCODER.getConfigurator().apply(config); } -} +} \ No newline at end of file From 32726f00b42e1bde3aca401169f72255cadaf75f Mon Sep 17 00:00:00 2001 From: yisha23 Date: Thu, 23 Nov 2023 20:52:54 +0200 Subject: [PATCH 03/18] added optimizeBusUtilization() --- .../robot/subsystems/sideshooter/SideShooterConstants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 2d51c076c..f71f893b1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -39,6 +39,7 @@ private static void configureShootingMotor() { config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; config.MotorOutput.Inverted = SHOOTING_INVERTED_VALUE; SHOOTING_MOTOR.getConfigurator().apply(config); + SHOOTING_MOTOR.optimizeBusUtilization(); } private static void configureAngleMotor(){ @@ -53,6 +54,7 @@ private static void configureAngleEncoder(){ config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; ANGLE_ENCODER.getConfigurator().apply(config); + ANGLE_ENCODER.optimizeBusUtilization(); } } \ No newline at end of file From c8dc826df9a9238caf22447494db61d2c2c0cbc2 Mon Sep 17 00:00:00 2001 From: yisha23 Date: Mon, 27 Nov 2023 14:25:38 +0200 Subject: [PATCH 04/18] created getSetTargetAngleCommand --- .../sideshooter/SideShooterConstants.java | 42 +++++++++- .../sideshooter/SideShooterSubsystem.java | 78 +++++++++++++++++++ 2 files changed, 118 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index f71f893b1..79c9ca219 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -1,5 +1,6 @@ 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; @@ -9,6 +10,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; public class SideShooterConstants { private static final int @@ -22,9 +26,27 @@ public class SideShooterConstants { private static final double ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final double VOLTAGE_COMPENSATION_VALUE = 0; + private static final double + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final boolean FOC_ENABLED = true; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION + ); + static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static final StatusSignal ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + private static final double + ANGLE_P = 0, + ANGLE_I = 0, + ANGLE_D = 0; + static final PIDController ANGLE_PID_CONTROLLER = new PIDController( + ANGLE_P, + ANGLE_I, + ANGLE_D + ); static { configureShootingMotor(); @@ -42,19 +64,35 @@ private static void configureShootingMotor() { SHOOTING_MOTOR.optimizeBusUtilization(); } - private static void configureAngleMotor(){ + private static void configureAngleMotor() { ANGEL_MOTOR.restoreFactoryDefaults(); ANGEL_MOTOR.setIdleMode(ANGLE_IDLE_MODE); ANGEL_MOTOR.setInverted(ANGLE_INVERTED); ANGEL_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_VALUE); } - private static void configureAngleEncoder(){ + private static void configureAngleEncoder() { CANcoderConfiguration config = new CANcoderConfiguration(); config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; ANGLE_ENCODER.getConfigurator().apply(config); + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGEL_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); } + public enum SideShooterState { + COLLECTION(Rotation2d.fromDegrees(0),0), + HIGH_STATE(Rotation2d.fromDegrees(0), 0), + MID_STATE(Rotation2d.fromDegrees(0), 0), + LOW_STATE(Rotation2d.fromDegrees(0), 0); + + final Rotation2d angle; + final double voltage; + + SideShooterState(Rotation2d angle, double voltage) { + this.angle = angle; + this.voltage = voltage; + } + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java new file mode 100644 index 000000000..a1bc66fb9 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java @@ -0,0 +1,78 @@ +package frc.trigon.robot.subsystems.sideshooter; + + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +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.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SideShooterSubsystem extends SubsystemBase { + private final static SideShooterSubsystem INSTANCE = new SideShooterSubsystem(); + private final static TalonFX SHOOTER_MOTOR = SideShooterConstants.SHOOTING_MOTOR; + private final static CANSparkMax ANGLE_MOTOR = SideShooterConstants.ANGEL_MOTOR; + private final static CANcoder ANGLE_ENCODER = SideShooterConstants.ANGLE_ENCODER; + private final VoltageOut ShootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); + public static SideShooterSubsystem getInstance() { + return INSTANCE; + } + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + + private SideShooterSubsystem() { + } + + private void setTargetShootingVoltage(double voltage) { + SHOOTER_MOTOR.setControl(ShootingVoltageRequest.withOutput(voltage)); + } + + private Rotation2d getAngleMotorPosition(){ + double positionRevolutions = SideShooterConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } + + private double getAngleMotorVelocity(){ + return SideShooterConstants.ANGEL_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); + } + + private double getAngleMotorProfileTime(){ + return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; + } + + private void generateAngleMotorProfile(Rotation2d targetAngle){ + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocity()) + ); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + } + + private void setTargetAngleFromProfile(){ + if (angleMotorProfile == null){ + ANGLE_MOTOR.stopMotor(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAngleMotorPosition().getDegrees(), targetState.position); + } + + public Command getSetTargetAngleCommand(Rotation2d targetAngle){ + return new FunctionalCommand( + ()-> generateAngleMotorProfile(targetAngle), + this::setTargetAngleFromProfile, + (interrupted) -> { + }, + ()-> false, + this + ); + } +} + From 04041e5617e59f8f800930a9e7aefab7865da5d9 Mon Sep 17 00:00:00 2001 From: yisha23 Date: Sun, 3 Dec 2023 13:06:57 +0200 Subject: [PATCH 05/18] fixed according to code rewiew --- .../sideshooter/SideShooterConstants.java | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 79c9ca219..c5b40c6da 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -8,6 +8,7 @@ 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.PIDController; @@ -23,21 +24,24 @@ public class SideShooterConstants { private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final InvertedValue SHOOTING_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; private static final double ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - private static final double VOLTAGE_COMPENSATION_VALUE = 0; - private static final double - MAX_ANGLE_VELOCITY = 0, - MAX_ANGLE_ACCELERATION = 0; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static final boolean FOC_ENABLED = true; static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); - static final boolean FOC_ENABLED = true; + private static final double + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( - MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION + MAX_ANGLE_VELOCITY, + MAX_ANGLE_ACCELERATION ); - static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); - static final StatusSignal ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + static final StatusSignal + ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); private static final double ANGLE_P = 0, ANGLE_I = 0, @@ -68,14 +72,16 @@ private static void configureAngleMotor() { ANGEL_MOTOR.restoreFactoryDefaults(); ANGEL_MOTOR.setIdleMode(ANGLE_IDLE_MODE); ANGEL_MOTOR.setInverted(ANGLE_INVERTED); - ANGEL_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_VALUE); + ANGEL_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); } private static void configureAngleEncoder() { CANcoderConfiguration config = new CANcoderConfiguration(); config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; ANGLE_ENCODER.getConfigurator().apply(config); + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGEL_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); From c70235387f07ffda4b572a3e1b90cf15e23bb870 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Sun, 3 Dec 2023 17:51:34 +0200 Subject: [PATCH 06/18] created SideShooterCommands --- .../subsystems/sideshooter/SideShooter.java | 70 +++++++++++++++++ .../sideshooter/SideShooterCommands.java | 45 +++++++++++ .../sideshooter/SideShooterSubsystem.java | 78 ------------------- 3 files changed, 115 insertions(+), 78 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java delete mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java new file mode 100644 index 000000000..b5ed1e3b6 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -0,0 +1,70 @@ +package frc.trigon.robot.subsystems.sideshooter; + + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +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.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SideShooter extends SubsystemBase { + private final static SideShooter INSTANCE = new SideShooter(); + private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; + private final CANSparkMax angelMotor = SideShooterConstants.ANGEL_MOTOR; + private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; + private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + public static SideShooter getInstance() { + return INSTANCE; + } + + private SideShooter() { + } + + void setTargetShootingVoltage(double voltage) { + shootingMotor.setControl(shootingVoltageRequest.withOutput(voltage)); + } + + void setTargetAngleFromProfile(){ + if (angleMotorProfile == null){ + angelMotor.stopMotor(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + double output = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); + angelMotor.setVoltage(output); + } + + void stopShooting(){ + shootingMotor.stopMotor(); + } + + private Rotation2d getAnglePosition(){ + double positionRevolutions = SideShooterConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } + + private double getAngleVelocity(){ + return SideShooterConstants.ANGEL_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); + } + + private double getAngleMotorProfileTime(){ + return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; + } + + void generateAngleMotorProfile(Rotation2d targetAngle){ + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocity()) + ); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + } +} + diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java new file mode 100644 index 000000000..3a0af783b --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,45 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.*; + +public class SideShooterCommands { + private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); + public static Command getSetTargetAngleCommand(Rotation2d targetAngle){ + return new FunctionalCommand( + ()-> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + SIDE_SHOOTER::setTargetAngleFromProfile, + (interrupted) -> { + }, + ()-> false, + SIDE_SHOOTER + ); + } + + public static Command getSetVoltageShootingCommand(double Voltage){ + return new StartEndCommand( + ()-> SideShooter.getInstance().setTargetShootingVoltage(Voltage), + ()-> SideShooter.getInstance().stopShooting(), + SIDE_SHOOTER + ); + } + + public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState state){ + if (byOrder){ + return new ParallelCommandGroup( + getSetTargetAngleCommand(state.angle), + getSetVoltageShootingCommand(state.voltage) + ); + }else { + return new SequentialCommandGroup( + getSetTargetAngleCommand(state.angle), + getSetVoltageShootingCommand(state.voltage) + ); + } + } +} + + + + + diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java deleted file mode 100644 index a1bc66fb9..000000000 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterSubsystem.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.trigon.robot.subsystems.sideshooter; - - -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -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.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class SideShooterSubsystem extends SubsystemBase { - private final static SideShooterSubsystem INSTANCE = new SideShooterSubsystem(); - private final static TalonFX SHOOTER_MOTOR = SideShooterConstants.SHOOTING_MOTOR; - private final static CANSparkMax ANGLE_MOTOR = SideShooterConstants.ANGEL_MOTOR; - private final static CANcoder ANGLE_ENCODER = SideShooterConstants.ANGLE_ENCODER; - private final VoltageOut ShootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); - public static SideShooterSubsystem getInstance() { - return INSTANCE; - } - private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGeneration; - - private SideShooterSubsystem() { - } - - private void setTargetShootingVoltage(double voltage) { - SHOOTER_MOTOR.setControl(ShootingVoltageRequest.withOutput(voltage)); - } - - private Rotation2d getAngleMotorPosition(){ - double positionRevolutions = SideShooterConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue(); - return Rotation2d.fromRotations(positionRevolutions); - } - - private double getAngleMotorVelocity(){ - return SideShooterConstants.ANGEL_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); - } - - private double getAngleMotorProfileTime(){ - return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; - } - - private void generateAngleMotorProfile(Rotation2d targetAngle){ - angleMotorProfile = new TrapezoidProfile( - SideShooterConstants.ANGLE_CONSTRAINTS, - new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocity()) - ); - - lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); - } - - private void setTargetAngleFromProfile(){ - if (angleMotorProfile == null){ - ANGLE_MOTOR.stopMotor(); - return; - } - - TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAngleMotorPosition().getDegrees(), targetState.position); - } - - public Command getSetTargetAngleCommand(Rotation2d targetAngle){ - return new FunctionalCommand( - ()-> generateAngleMotorProfile(targetAngle), - this::setTargetAngleFromProfile, - (interrupted) -> { - }, - ()-> false, - this - ); - } -} - From 50c5418b82dbc168f74d492fd24727e050fef162 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Sun, 3 Dec 2023 18:41:24 +0200 Subject: [PATCH 07/18] fixed according to code rewiew --- .../subsystems/sideshooter/SideShooter.java | 48 ++++++++++--------- .../sideshooter/SideShooterCommands.java | 47 ++++++++---------- .../sideshooter/SideShooterConstants.java | 5 +- 3 files changed, 51 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index b5ed1e3b6..61a8f5b35 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -1,10 +1,10 @@ package frc.trigon.robot.subsystems.sideshooter; - import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; @@ -18,6 +18,7 @@ public class SideShooter extends SubsystemBase { private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGeneration; + public static SideShooter getInstance() { return INSTANCE; } @@ -25,46 +26,49 @@ public static SideShooter getInstance() { private SideShooter() { } - void setTargetShootingVoltage(double voltage) { - shootingMotor.setControl(shootingVoltageRequest.withOutput(voltage)); + void setTargetShootingVoltage(double targetVoltage) { + shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); + } + + void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocity()) + ); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } - void setTargetAngleFromProfile(){ - if (angleMotorProfile == null){ + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { angelMotor.stopMotor(); return; } - TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); double output = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); angelMotor.setVoltage(output); } - void stopShooting(){ + void stopShooting() { shootingMotor.stopMotor(); } - private Rotation2d getAnglePosition(){ + boolean atAngle(Rotation2d angle){ + return getAnglePosition() == angle; + } + + private Rotation2d getAnglePosition() { double positionRevolutions = SideShooterConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolutions); } - private double getAngleVelocity(){ + private double getAngleVelocity() { return SideShooterConstants.ANGEL_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); } - private double getAngleMotorProfileTime(){ + private double getAngleProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; } - - void generateAngleMotorProfile(Rotation2d targetAngle){ - angleMotorProfile = new TrapezoidProfile( - SideShooterConstants.ANGLE_CONSTRAINTS, - new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocity()) - ); - - lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); - } -} - +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java index 3a0af783b..3ae42a6da 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -5,41 +5,36 @@ public class SideShooterCommands { private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); - public static Command getSetTargetAngleCommand(Rotation2d targetAngle){ + + public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState targetState) { + if (!byOrder) { + return new ParallelCommandGroup( + getSetTargetAngleCommand(targetState.angle), + getSetTargetShootingVoltageCommand(targetState.voltage) + ); + } + return new SequentialCommandGroup( + getSetTargetAngleCommand(targetState.angle).until(()-> SIDE_SHOOTER.atAngle(targetState.angle)), + getSetTargetShootingVoltageCommand(targetState.voltage) + ); + } + + public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( - ()-> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), SIDE_SHOOTER::setTargetAngleFromProfile, (interrupted) -> { }, - ()-> false, + () -> false, SIDE_SHOOTER ); } - public static Command getSetVoltageShootingCommand(double Voltage){ + public static Command getSetTargetShootingVoltageCommand(double targetVoltage) { return new StartEndCommand( - ()-> SideShooter.getInstance().setTargetShootingVoltage(Voltage), - ()-> SideShooter.getInstance().stopShooting(), + () -> SIDE_SHOOTER.setTargetShootingVoltage(targetVoltage), + SIDE_SHOOTER::stopShooting, SIDE_SHOOTER ); } - - public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState state){ - if (byOrder){ - return new ParallelCommandGroup( - getSetTargetAngleCommand(state.angle), - getSetVoltageShootingCommand(state.voltage) - ); - }else { - return new SequentialCommandGroup( - getSetTargetAngleCommand(state.angle), - getSetVoltageShootingCommand(state.voltage) - ); - } - } -} - - - - - +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index c5b40c6da..cac763d00 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -32,6 +32,7 @@ public class SideShooterConstants { static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + private static final double MAX_ANGLE_VELOCITY = 0, MAX_ANGLE_ACCELERATION = 0; @@ -39,9 +40,11 @@ public class SideShooterConstants { MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION ); + static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + private static final double ANGLE_P = 0, ANGLE_I = 0, @@ -88,7 +91,7 @@ private static void configureAngleEncoder() { } public enum SideShooterState { - COLLECTION(Rotation2d.fromDegrees(0),0), + COLLECTION(Rotation2d.fromDegrees(0), 0), HIGH_STATE(Rotation2d.fromDegrees(0), 0), MID_STATE(Rotation2d.fromDegrees(0), 0), LOW_STATE(Rotation2d.fromDegrees(0), 0); From 0b793c0eea6661a6511c0357f41a11873fcfde85 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 4 Dec 2023 18:28:19 +0200 Subject: [PATCH 08/18] finished subsystem --- .../robot/subsystems/sideshooter/SideShooter.java | 6 ++---- .../subsystems/sideshooter/SideShooterCommands.java | 13 +++++++++---- .../java/frc/trigon/robot/utilities/Commands.java | 4 ++-- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index 61a8f5b35..1fe828b20 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -1,10 +1,8 @@ package frc.trigon.robot.subsystems.sideshooter; import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; @@ -14,10 +12,10 @@ public class SideShooter extends SubsystemBase { private final static SideShooter INSTANCE = new SideShooter(); private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; private final CANSparkMax angelMotor = SideShooterConstants.ANGEL_MOTOR; - private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGeneration; + private double angleTolerance = 1; public static SideShooter getInstance() { return INSTANCE; @@ -56,7 +54,7 @@ void stopShooting() { } boolean atAngle(Rotation2d angle){ - return getAnglePosition() == angle; + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < angleTolerance; } private Rotation2d getAnglePosition() { diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java index 3ae42a6da..d15e500e6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -1,7 +1,12 @@ package frc.trigon.robot.subsystems.sideshooter; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.trigon.robot.utilities.Commands; public class SideShooterCommands { private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); @@ -9,19 +14,19 @@ public class SideShooterCommands { public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState targetState) { if (!byOrder) { return new ParallelCommandGroup( - getSetTargetAngleCommand(targetState.angle), + Commands.removeRequirements(getSetTargetAngleCommand(targetState.angle)), getSetTargetShootingVoltageCommand(targetState.voltage) ); } return new SequentialCommandGroup( - getSetTargetAngleCommand(targetState.angle).until(()-> SIDE_SHOOTER.atAngle(targetState.angle)), + Commands.removeRequirements(getSetTargetAngleCommand(targetState.angle).until(() -> SIDE_SHOOTER.atAngle(targetState.angle))), getSetTargetShootingVoltageCommand(targetState.voltage) ); } public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( - () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + () -> SIDE_SHOOTER.generateAngleMotorProfile(targetAngle), SIDE_SHOOTER::setTargetAngleFromProfile, (interrupted) -> { }, diff --git a/src/main/java/frc/trigon/robot/utilities/Commands.java b/src/main/java/frc/trigon/robot/utilities/Commands.java index 507bd8d2d..1e4fcce9a 100644 --- a/src/main/java/frc/trigon/robot/utilities/Commands.java +++ b/src/main/java/frc/trigon/robot/utilities/Commands.java @@ -1,10 +1,10 @@ package frc.trigon.robot.utilities; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; public class Commands { - public static Command removeRequirements(CommandBase command) { + public static Command removeRequirements(Command command) { return new FunctionalCommand( command::initialize, command::execute, From 0e6737289d5602919219a27d56de8c52160648c6 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 4 Dec 2023 20:09:28 +0200 Subject: [PATCH 09/18] created ArmConstants --- .../robot/subsystems/arm/ArmConstants.java | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 000000000..779c97565 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,133 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.trigon.robot.utilities.Conversions; + +public class ArmConstants { + private static final int + MASTER_ELEVATOR_MOTOR_ID = 0, + FOLLOWER_ELEVATOR_MOTOR_ID = 0, + MASTER_ANGLE_MOTOR_ID = 0, + FOLLOWER_ANGLE_MOTOR_ID = 0, + ANGLE_ENCODER_ID = 0, + ELEVATOR_ENCODER_ID = 0; + private static final CANSparkMax.IdleMode + MASTER_ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + MASTER_ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final boolean + MASTER_ANGLE_INVERTED = false, + FOLLOWER_ANGLE_INVERTED = false, + MASTER_ELEVATOR_INVERTED = false, + FOLLOWER_ELEVATOR_INVERTED = false, + ELEVATOR_ENCODER_PHASE = false; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; + private static final double + ANGLE_ENCODER_OFFSET = 0, + ELEVATOR_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + static final CANSparkMax + MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); + + private static final double + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, + MAX_ANGLE_ACCELERATION + ); + + static final StatusSignal + ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + private static final double + ANGLE_P = 0, + ANGLE_I = 0, + ANGLE_D = 0; + static final PIDController ANGLE_PID_CONTROLLER = new PIDController( + ANGLE_P, + ANGLE_I, + ANGLE_D + ); + + static { + configureAngleMotors(); + configureElevatorMotors(); + configureAngleEncoder(); + configureElevatorEncoder(); + } + + private static void configureAngleMotors() { + MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_IDLE_MODE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); + MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureElevatorMotors() { + MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ELEVATOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ELEVATOR_IDLE_MODE); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; + ANGLE_ENCODER.getConfigurator().apply(config); + + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGEL_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + private static void configureElevatorEncoder() { + ELEVATOR_ENCODER.configFactoryDefault(); + ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute); + ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); + ELEVATOR_ENCODER.setSelectedSensorPosition(Conversions.offsetRead(ELEVATOR_ENCODER.getSelectedSensorPosition(), ELEVATOR_ENCODER_OFFSET)); + } + + public enum ArmState { + CONE_COLLECTION(Rotation2d.fromDegrees(0), 0), + CONE_HIGH_STATE(Rotation2d.fromDegrees(0), 0), + CONE_MID_STATE(Rotation2d.fromDegrees(0), 0), + CONE_LOW_STATE(Rotation2d.fromDegrees(0), 0); + + final Rotation2d angle; + final double elevatorPosition; + + ArmState(Rotation2d angle, double elevatorPosition) { + this.angle = angle; + this.elevatorPosition = elevatorPosition; + } + } +} From e9378440a822c323f7b5711f7b59242a343528a8 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 4 Dec 2023 20:10:15 +0200 Subject: [PATCH 10/18] reformat class --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 779c97565..d6efafe7e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -121,7 +121,7 @@ public enum ArmState { CONE_HIGH_STATE(Rotation2d.fromDegrees(0), 0), CONE_MID_STATE(Rotation2d.fromDegrees(0), 0), CONE_LOW_STATE(Rotation2d.fromDegrees(0), 0); - + final Rotation2d angle; final double elevatorPosition; From 6ec507adb81671261ee62e29bc48904970e19cb3 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Tue, 5 Dec 2023 16:38:01 +0200 Subject: [PATCH 11/18] fixed according to code review --- .../robot/subsystems/arm/ArmConstants.java | 22 +++++++++---------- .../subsystems/sideshooter/SideShooter.java | 10 ++++----- .../sideshooter/SideShooterConstants.java | 18 +++++++-------- 3 files changed, 24 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index d6efafe7e..b1008bd21 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -23,10 +23,8 @@ public class ArmConstants { ANGLE_ENCODER_ID = 0, ELEVATOR_ENCODER_ID = 0; private static final CANSparkMax.IdleMode - MASTER_ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, - MASTER_ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final boolean MASTER_ANGLE_INVERTED = false, FOLLOWER_ANGLE_INVERTED = false, @@ -56,8 +54,8 @@ public class ArmConstants { ); static final StatusSignal - ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), - ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); private static final double ANGLE_P = 0, ANGLE_I = 0, @@ -78,8 +76,8 @@ public class ArmConstants { private static void configureAngleMotors() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); - MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_IDLE_MODE); - FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_IDLE_MODE); + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); @@ -89,8 +87,8 @@ private static void configureAngleMotors() { private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); - MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ELEVATOR_IDLE_MODE); - FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ELEVATOR_IDLE_MODE); + MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); @@ -104,8 +102,8 @@ private static void configureAngleEncoder() { config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; ANGLE_ENCODER.getConfigurator().apply(config); - ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); - ANGEL_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index 1fe828b20..e6b2d48f8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -11,7 +11,7 @@ public class SideShooter extends SubsystemBase { private final static SideShooter INSTANCE = new SideShooter(); private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; - private final CANSparkMax angelMotor = SideShooterConstants.ANGEL_MOTOR; + private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGeneration; @@ -40,13 +40,13 @@ void generateAngleMotorProfile(Rotation2d targetAngle) { void setTargetAngleFromProfile() { if (angleMotorProfile == null) { - angelMotor.stopMotor(); + angleMotor.stopMotor(); return; } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); double output = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); - angelMotor.setVoltage(output); + angleMotor.setVoltage(output); } void stopShooting() { @@ -58,12 +58,12 @@ boolean atAngle(Rotation2d angle){ } private Rotation2d getAnglePosition() { - double positionRevolutions = SideShooterConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue(); + double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolutions); } private double getAngleVelocity() { - return SideShooterConstants.ANGEL_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); + return SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); } private double getAngleProfileTime() { diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index cac763d00..82bf87956 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -31,7 +31,7 @@ public class SideShooterConstants { static final boolean FOC_ENABLED = true; static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); - static final CANSparkMax ANGEL_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); private static final double MAX_ANGLE_VELOCITY = 0, @@ -42,8 +42,8 @@ public class SideShooterConstants { ); static final StatusSignal - ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), - ANGEL_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); private static final double ANGLE_P = 0, @@ -72,10 +72,10 @@ private static void configureShootingMotor() { } private static void configureAngleMotor() { - ANGEL_MOTOR.restoreFactoryDefaults(); - ANGEL_MOTOR.setIdleMode(ANGLE_IDLE_MODE); - ANGEL_MOTOR.setInverted(ANGLE_INVERTED); - ANGEL_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + ANGLE_MOTOR.restoreFactoryDefaults(); + ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + ANGLE_MOTOR.setInverted(ANGLE_INVERTED); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); } private static void configureAngleEncoder() { @@ -85,8 +85,8 @@ private static void configureAngleEncoder() { config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; ANGLE_ENCODER.getConfigurator().apply(config); - ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); - ANGEL_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); } From ab18038e62bd3414ee5db56783556f64666268ea Mon Sep 17 00:00:00 2001 From: yishay23 Date: Tue, 5 Dec 2023 20:25:09 +0200 Subject: [PATCH 12/18] fixed according to code reviwe --- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 3 ++- .../robot/subsystems/sideshooter/SideShooter.java | 10 +++++----- .../subsystems/sideshooter/SideShooterCommands.java | 8 +++++++- .../subsystems/sideshooter/SideShooterConstants.java | 1 + 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index b1008bd21..49f12919e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -31,8 +31,8 @@ public class ArmConstants { MASTER_ELEVATOR_INVERTED = false, FOLLOWER_ELEVATOR_INVERTED = false, ELEVATOR_ENCODER_PHASE = false; - private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final double ANGLE_ENCODER_OFFSET = 0, ELEVATOR_ENCODER_OFFSET = 0; @@ -56,6 +56,7 @@ public class ArmConstants { static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + private static final double ANGLE_P = 0, ANGLE_I = 0, diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index e6b2d48f8..7897286b0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; 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 static SideShooter INSTANCE = new SideShooter(); @@ -15,7 +16,6 @@ public class SideShooter extends SubsystemBase { private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGeneration; - private double angleTolerance = 1; public static SideShooter getInstance() { return INSTANCE; @@ -32,7 +32,7 @@ void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocity()) + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityInDegreesPerSeconds()) ); lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); @@ -54,7 +54,7 @@ void stopShooting() { } boolean atAngle(Rotation2d angle){ - return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < angleTolerance; + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.angleTolerance; } private Rotation2d getAnglePosition() { @@ -62,8 +62,8 @@ private Rotation2d getAnglePosition() { return Rotation2d.fromRotations(positionRevolutions); } - private double getAngleVelocity() { - return SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue(); + private double getAngleVelocityInDegreesPerSeconds() { + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } private double getAngleProfileTime() { diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java index d15e500e6..84ce9c504 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -11,6 +11,12 @@ public class SideShooterCommands { private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); + /** + * goes to the wanted state. + * @param byOrder causing it to be in parallel or one after the other + * @param targetState the wanted state + * @return a command that goes to the wanted state + */ public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState targetState) { if (!byOrder) { return new ParallelCommandGroup( @@ -19,7 +25,7 @@ public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConst ); } return new SequentialCommandGroup( - Commands.removeRequirements(getSetTargetAngleCommand(targetState.angle).until(() -> SIDE_SHOOTER.atAngle(targetState.angle))), + getSetTargetAngleCommand(targetState.angle).until(() -> SIDE_SHOOTER.atAngle(targetState.angle)), getSetTargetShootingVoltageCommand(targetState.voltage) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 82bf87956..636a73384 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -28,6 +28,7 @@ public class SideShooterConstants { private static final double ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static double angleTolerance = 1; static final boolean FOC_ENABLED = true; static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); From c67882feebaa98ad5ef7234ca82ed100a6b62a0a Mon Sep 17 00:00:00 2001 From: yishay23 Date: Tue, 5 Dec 2023 22:29:12 +0200 Subject: [PATCH 13/18] added Arm and ArmCommands --- .../frc/trigon/robot/subsystems/arm/Arm.java | 119 ++++++++++++++++++ .../robot/subsystems/arm/ArmCommands.java | 45 +++++++ .../robot/subsystems/arm/ArmConstants.java | 25 +++- 3 files changed, 187 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/Arm.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java new file mode 100644 index 000000000..f12c69fb4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,119 @@ +package frc.trigon.robot.subsystems.arm; + + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.subsystems.sideshooter.SideShooterConstants; +import frc.trigon.robot.utilities.Conversions; + +public class Arm extends SubsystemBase { + private final static Arm INSTANCE = new Arm(); + private final CANSparkMax + masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + masterAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + followerAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR; + private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + + private TrapezoidProfile elevatorMotorProfile = null; + private double lastElevatorMotorProfileGeneration; + + public static Arm getInstance() { + return INSTANCE; + } + + private Arm() { + } + + void generateElevatorMotorProfile(double targetElevatorMeters) { + elevatorMotorProfile = new TrapezoidProfile( + ArmConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetElevatorMeters, 0), + new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityInMetersPerSeconds()) + ); + } + + void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile( + ArmConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityInDegreesPerSeconds()) + ); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + } + + private Rotation2d getAnglePosition() { + double positionRevolutions = ArmConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } + + private double getElevatorPositionMeters() { + double elevatorPositionMagTicks = elevatorEncoder.getSelectedSensorPosition(); + double elevatorPositionRevolution = Conversions.magTicksToRevolutions(elevatorPositionMagTicks); + return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; + } + + private double getAngleVelocityInDegreesPerSeconds() { + return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + } + + private double getElevatorVelocityInMetersPerSeconds(){ + double elevatorVelocityMagTicksPer100ms = elevatorEncoder.getSelectedSensorVelocity(); + double elevatorVelocityMagTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorVelocityMagTicksPer100ms); + double elevatorVelocityRevolutionPerSecond = Conversions.magTicksToRevolutions(elevatorVelocityMagTicksPerSecond); + return elevatorVelocityRevolutionPerSecond / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; + } + + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); + double output = ArmConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); + masterAngleMotor.setVoltage(output); + followerAngleMotor.setVoltage(output); + } + + void setTargetElevatorPositionFromProfile() { + if (angleMotorProfile == null) { + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); + return; + } + + TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorProfileTime()); + double output = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate(getElevatorPositionMeters(), targetState.position); + masterElevatorMotor.setVoltage(output); + followerElevatorMotor.setVoltage(output); + } + + boolean atAngle(Rotation2d angle){ + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE; + } + + boolean atElevatorTargetMeters(double targetElevatorMeters){ + return Math.abs(targetElevatorMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE; + } + + boolean isElevatorOpening(double targetElevatorMeters){ + return targetElevatorMeters > getElevatorPositionMeters(); + } + + private double getAngleProfileTime() { + return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; + } + + private double getElevatorProfileTime(){ + return Timer.getFPGATimestamp() - lastElevatorMotorProfileGeneration; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java new file mode 100644 index 000000000..735fcc721 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -0,0 +1,45 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ArmCommands { + private static final Arm ARM = Arm.getInstance(); + + public static Command setTargetState(Rotation2d targetAngle, double targetElevatorMeters) { + if (ARM.isElevatorOpening(targetElevatorMeters)) { + return new SequentialCommandGroup( + getSetTargetAngleCommand(targetAngle).until(() -> ARM.atAngle(targetAngle)), + getSetTargetElevatorPositionCommand(targetElevatorMeters) + ); + } + return new SequentialCommandGroup( + getSetTargetElevatorPositionCommand(targetElevatorMeters).until(()-> ARM.atElevatorTargetMeters(targetElevatorMeters)), + getSetTargetAngleCommand(targetAngle) + ); + } + + public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> ARM.generateAngleMotorProfile(targetAngle), + ARM::setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + ARM + ); + } + + public static Command getSetTargetElevatorPositionCommand(double targetElevatorMeters) { + return new FunctionalCommand( + () -> ARM.generateElevatorMotorProfile(targetElevatorMeters), + ARM::setTargetElevatorPositionFromProfile, + (interrupted) -> { + }, + () -> false, + ARM + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 49f12919e..602f60fce 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -44,6 +44,27 @@ public class ArmConstants { FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); + static final double ELEVATOR_METERS_PER_REVOLUTION = 1; + static final double ANGLE_TOLERANCE = 1; + static final double ELEVATOR_TOLERANCE = 1; + + private static final double + MAX_ELEVATOR_VELOCITY = 0, + MAX_ELEVATOR_ACCELERATION = 0; + static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ELEVATOR_VELOCITY, + MAX_ELEVATOR_ACCELERATION + ); + + private static final double + ELEVATOR_P = 0, + ELEVATOR_I = 0, + ELEVATOR_D = 0; + static final PIDController ELEVATOR_PID_CONTROLLER = new PIDController( + ELEVATOR_P, + ELEVATOR_I, + ELEVATOR_D + ); private static final double MAX_ANGLE_VELOCITY = 0, @@ -122,11 +143,11 @@ public enum ArmState { CONE_LOW_STATE(Rotation2d.fromDegrees(0), 0); final Rotation2d angle; - final double elevatorPosition; + final double elevatorMeters; ArmState(Rotation2d angle, double elevatorPosition) { this.angle = angle; - this.elevatorPosition = elevatorPosition; + this.elevatorMeters = elevatorPosition; } } } From ca4f95ece460f1fce387b24896189e8f3842cf40 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 18 Dec 2023 17:59:54 +0200 Subject: [PATCH 14/18] added feedforward --- .../frc/trigon/robot/subsystems/arm/Arm.java | 60 +++++++++++++++---- .../robot/subsystems/arm/ArmCommands.java | 2 +- .../robot/subsystems/arm/ArmConstants.java | 59 ++++++++++++------ .../subsystems/sideshooter/SideShooter.java | 21 ++++++- .../sideshooter/SideShooterCommands.java | 13 ++-- .../sideshooter/SideShooterConstants.java | 15 ++++- 6 files changed, 127 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index f12c69fb4..09ed21378 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -5,9 +5,9 @@ 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.subsystems.sideshooter.SideShooterConstants; import frc.trigon.robot.utilities.Conversions; public class Arm extends SubsystemBase { @@ -19,9 +19,8 @@ public class Arm extends SubsystemBase { followerAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR; private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGeneration; - private TrapezoidProfile elevatorMotorProfile = null; + private double lastAngleMotorProfileGeneration; private double lastElevatorMotorProfileGeneration; public static Arm getInstance() { @@ -35,15 +34,17 @@ void generateElevatorMotorProfile(double targetElevatorMeters) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetElevatorMeters, 0), - new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityInMetersPerSeconds()) + new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityMetersPerSeconds()) ); + + lastElevatorMotorProfileGeneration = Timer.getFPGATimestamp(); } void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityInDegreesPerSeconds()) + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSeconds()) ); lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); @@ -60,11 +61,11 @@ private double getElevatorPositionMeters() { return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; } - private double getAngleVelocityInDegreesPerSeconds() { + private double getAngleVelocityDegreesPerSeconds() { return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } - private double getElevatorVelocityInMetersPerSeconds(){ + private double getElevatorVelocityMetersPerSeconds(){ double elevatorVelocityMagTicksPer100ms = elevatorEncoder.getSelectedSensorVelocity(); double elevatorVelocityMagTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorVelocityMagTicksPer100ms); double elevatorVelocityRevolutionPerSecond = Conversions.magTicksToRevolutions(elevatorVelocityMagTicksPerSecond); @@ -79,9 +80,7 @@ void setTargetAngleFromProfile() { } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); - double output = ArmConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); - masterAngleMotor.setVoltage(output); - followerAngleMotor.setVoltage(output); + setAngleMotorsVoltage(calculateAngleOutput(targetState)); } void setTargetElevatorPositionFromProfile() { @@ -92,9 +91,44 @@ void setTargetElevatorPositionFromProfile() { } TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorProfileTime()); - double output = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate(getElevatorPositionMeters(), targetState.position); - masterElevatorMotor.setVoltage(output); - followerElevatorMotor.setVoltage(output); + setElevatorMotorsVoltage(calculateElevatorOutput(targetState)); + } + + private double calculateAngleOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + + return pidOutput + feedforward; + } + + private double calculateElevatorOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( + targetState.velocity + ); + + return pidOutput + feedforward; + } + + private void setAngleMotorsVoltage(double outputVoltage){ + masterAngleMotor.setVoltage(outputVoltage); + followerAngleMotor.setVoltage(outputVoltage); + } + + private void setElevatorMotorsVoltage(double outputVoltage){ + masterElevatorMotor.setVoltage(outputVoltage); + followerElevatorMotor.setVoltage(outputVoltage); } boolean atAngle(Rotation2d angle){ diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 735fcc721..c53d75078 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -8,7 +8,7 @@ public class ArmCommands { private static final Arm ARM = Arm.getInstance(); - public static Command setTargetState(Rotation2d targetAngle, double targetElevatorMeters) { + public static Command getSetTargetStateCommand(Rotation2d targetAngle, double targetElevatorMeters) { if (ARM.isElevatorOpening(targetElevatorMeters)) { return new SequentialCommandGroup( getSetTargetAngleCommand(targetAngle).until(() -> ARM.atAngle(targetAngle)), diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 602f60fce..160e734eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -9,6 +9,8 @@ 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.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -51,24 +53,13 @@ public class ArmConstants { private static final double MAX_ELEVATOR_VELOCITY = 0, MAX_ELEVATOR_ACCELERATION = 0; + private static final double + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION ); - - private static final double - ELEVATOR_P = 0, - ELEVATOR_I = 0, - ELEVATOR_D = 0; - static final PIDController ELEVATOR_PID_CONTROLLER = new PIDController( - ELEVATOR_P, - ELEVATOR_I, - ELEVATOR_D - ); - - private static final double - MAX_ANGLE_VELOCITY = 0, - MAX_ANGLE_ACCELERATION = 0; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION @@ -88,6 +79,40 @@ public class ArmConstants { ANGLE_D ); + private static final double + ELEVATOR_P = 0, + ELEVATOR_I = 0, + ELEVATOR_D = 0; + static final PIDController ELEVATOR_PID_CONTROLLER = new PIDController( + ELEVATOR_P, + ELEVATOR_I, + ELEVATOR_D + ); + + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, + ANGLE_MOTOR_KV, + ANGLE_MOTOR_KA, + ANGLE_MOTOR_KG + ); + + private static final double + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward( + ELEVATOR_MOTOR_KS, + ELEVATOR_MOTOR_KG, + ELEVATOR_MOTOR_KV, + ELEVATOR_MOTOR_KA + ); + static { configureAngleMotors(); configureElevatorMotors(); @@ -143,11 +168,11 @@ public enum ArmState { CONE_LOW_STATE(Rotation2d.fromDegrees(0), 0); final Rotation2d angle; - final double elevatorMeters; + final double elevatorPositionMeters; - ArmState(Rotation2d angle, double elevatorPosition) { + ArmState(Rotation2d angle, double elevatorPositionMeters) { this.angle = angle; - this.elevatorMeters = elevatorPosition; + this.elevatorPositionMeters = elevatorPositionMeters; } } } diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index 7897286b0..d890dafaf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -5,8 +5,10 @@ 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.subsystems.arm.ArmConstants; import frc.trigon.robot.utilities.Conversions; public class SideShooter extends SubsystemBase { @@ -45,8 +47,7 @@ void setTargetAngleFromProfile() { } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); - double output = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(getAnglePosition().getDegrees(), targetState.position); - angleMotor.setVoltage(output); + angleMotor.setVoltage(calculateAngleOutput(targetState)); } void stopShooting() { @@ -54,7 +55,21 @@ void stopShooting() { } boolean atAngle(Rotation2d angle){ - return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.angleTolerance; + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.ANGLE_TOLERANCE; + } + + private double calculateAngleOutput(TrapezoidProfile.State targetState){ + double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = SideShooterConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + + return pidOutput + feedforward; } private Rotation2d getAnglePosition() { diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java index 84ce9c504..e31af97ee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -1,21 +1,18 @@ 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.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.utilities.Commands; public class SideShooterCommands { private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); /** - * goes to the wanted state. - * @param byOrder causing it to be in parallel or one after the other + * Creates a command that goes to the wanted state. + * + * @param byOrder should the side shooter move parallel commands or sequential commands * @param targetState the wanted state - * @return a command that goes to the wanted state + * @return the command */ public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState targetState) { if (!byOrder) { diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 636a73384..12a1680c9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -11,6 +11,7 @@ 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; @@ -28,11 +29,11 @@ public class SideShooterConstants { private static final double ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final double VOLTAGE_COMPENSATION_SATURATION = 12; - static double angleTolerance = 1; static final boolean FOC_ENABLED = true; static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final double ANGLE_TOLERANCE = 1; private static final double MAX_ANGLE_VELOCITY = 0, @@ -56,6 +57,18 @@ public class SideShooterConstants { ANGLE_D ); + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, + ANGLE_MOTOR_KV, + ANGLE_MOTOR_KA, + ANGLE_MOTOR_KG + ); + static { configureShootingMotor(); configureAngleMotor(); From 847e2bf5e96b6d21fd9250684c3c4f2719e9b27a Mon Sep 17 00:00:00 2001 From: yishay23 Date: Wed, 20 Dec 2023 18:26:02 +0200 Subject: [PATCH 15/18] fixed according code review --- .../frc/trigon/robot/subsystems/arm/Arm.java | 64 ++++++++++--------- .../robot/subsystems/arm/ArmCommands.java | 28 +++++--- .../robot/subsystems/arm/ArmConstants.java | 51 ++++++++------- 3 files changed, 76 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 09ed21378..4c85607a0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -18,10 +18,12 @@ public class Arm extends SubsystemBase { masterAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, followerAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR; private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; - private TrapezoidProfile angleMotorProfile = null; - private TrapezoidProfile elevatorMotorProfile = null; - private double lastAngleMotorProfileGeneration; - private double lastElevatorMotorProfileGeneration; + private TrapezoidProfile + angleMotorProfile = null, + elevatorMotorProfile = null; + private double + lastAngleMotorProfileGeneration, + lastElevatorMotorProfileGeneration; public static Arm getInstance() { return INSTANCE; @@ -30,19 +32,19 @@ public static Arm getInstance() { private Arm() { } - void generateElevatorMotorProfile(double targetElevatorMeters) { + void generateElevatorMotorProfile(double targetElevatorPositionMeters, double elevatorSpeedPercentage) { elevatorMotorProfile = new TrapezoidProfile( - ArmConstants.ANGLE_CONSTRAINTS, - new TrapezoidProfile.State(targetElevatorMeters, 0), + Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, elevatorSpeedPercentage), + new TrapezoidProfile.State(targetElevatorPositionMeters, 0), new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityMetersPerSeconds()) ); lastElevatorMotorProfileGeneration = Timer.getFPGATimestamp(); } - void generateAngleMotorProfile(Rotation2d targetAngle) { + void generateAngleMotorProfile(Rotation2d targetAngle, double angleSpeedPercentage) { angleMotorProfile = new TrapezoidProfile( - ArmConstants.ANGLE_CONSTRAINTS, + Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, angleSpeedPercentage), new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSeconds()) ); @@ -94,20 +96,6 @@ void setTargetElevatorPositionFromProfile() { setElevatorMotorsVoltage(calculateElevatorOutput(targetState)); } - private double calculateAngleOutput(TrapezoidProfile.State targetState){ - double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( - getAnglePosition().getDegrees(), - targetState.position - ); - - double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( - Units.degreesToRadians(targetState.position), - Units.degreesToRadians(targetState.velocity) - ); - - return pidOutput + feedforward; - } - private double calculateElevatorOutput(TrapezoidProfile.State targetState){ double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( getAnglePosition().getDegrees(), @@ -126,23 +114,37 @@ private void setAngleMotorsVoltage(double outputVoltage){ followerAngleMotor.setVoltage(outputVoltage); } - private void setElevatorMotorsVoltage(double outputVoltage){ - masterElevatorMotor.setVoltage(outputVoltage); - followerElevatorMotor.setVoltage(outputVoltage); - } - boolean atAngle(Rotation2d angle){ - return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE; + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE_DEGREES; } - boolean atElevatorTargetMeters(double targetElevatorMeters){ - return Math.abs(targetElevatorMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE; + boolean atElevatorMeters(double ElevatorPositionMeters){ + return Math.abs(ElevatorPositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_DEGREES; } boolean isElevatorOpening(double targetElevatorMeters){ return targetElevatorMeters > getElevatorPositionMeters(); } + private double calculateAngleOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + + return pidOutput + feedforward; + } + + private void setElevatorMotorsVoltage(double outputVoltage){ + masterElevatorMotor.setVoltage(outputVoltage); + followerElevatorMotor.setVoltage(outputVoltage); + } + private double getAngleProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index c53d75078..94ac885b8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -8,22 +8,30 @@ public class ArmCommands { private static final Arm ARM = Arm.getInstance(); - public static Command getSetTargetStateCommand(Rotation2d targetAngle, double targetElevatorMeters) { - if (ARM.isElevatorOpening(targetElevatorMeters)) { + public static Command getSetTargetStateCommand(ArmConstants.ArmState state){ + return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, 100, 100); + } + + public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentages, double angleSpeedPercentages){ + return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters,elevatorSpeedPercentages, angleSpeedPercentages); + } + + public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentage) { + if (ARM.isElevatorOpening(targetElevatorPositionMeters)) { return new SequentialCommandGroup( - getSetTargetAngleCommand(targetAngle).until(() -> ARM.atAngle(targetAngle)), - getSetTargetElevatorPositionCommand(targetElevatorMeters) + getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)), + getSetTargetElevatorPositionMetersCommand(targetElevatorPositionMeters,elevatorSpeedPercentages) ); } return new SequentialCommandGroup( - getSetTargetElevatorPositionCommand(targetElevatorMeters).until(()-> ARM.atElevatorTargetMeters(targetElevatorMeters)), - getSetTargetAngleCommand(targetAngle) + getSetTargetElevatorPositionMetersCommand(targetElevatorPositionMeters, elevatorSpeedPercentages).until(()-> ARM.atElevatorMeters(targetElevatorPositionMeters)), + getSetTargetAngleCommand(targetAngle, angleSpeedPercentage) ); } - public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { + public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double angleSpeedPercentage) { return new FunctionalCommand( - () -> ARM.generateAngleMotorProfile(targetAngle), + () -> ARM.generateAngleMotorProfile(targetAngle, angleSpeedPercentage), ARM::setTargetAngleFromProfile, (interrupted) -> { }, @@ -32,9 +40,9 @@ public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { ); } - public static Command getSetTargetElevatorPositionCommand(double targetElevatorMeters) { + public static Command getSetTargetElevatorPositionMetersCommand(double targetElevatorPositionMeters, double elevatorSpeedPercentage) { return new FunctionalCommand( - () -> ARM.generateElevatorMotorProfile(targetElevatorMeters), + () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, elevatorSpeedPercentage), ARM::setTargetElevatorPositionFromProfile, (interrupted) -> { }, diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 160e734eb..235e27237 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -47,23 +47,23 @@ public class ArmConstants { static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); static final double ELEVATOR_METERS_PER_REVOLUTION = 1; - static final double ANGLE_TOLERANCE = 1; - static final double ELEVATOR_TOLERANCE = 1; + static final double + ANGLE_TOLERANCE_DEGREES = 1, + ELEVATOR_TOLERANCE_DEGREES = 1; private static final double MAX_ELEVATOR_VELOCITY = 0, - MAX_ELEVATOR_ACCELERATION = 0; - private static final double + MAX_ELEVATOR_ACCELERATION = 0, MAX_ANGLE_VELOCITY = 0, MAX_ANGLE_ACCELERATION = 0; static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION - ); - static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( - MAX_ANGLE_VELOCITY, - MAX_ANGLE_ACCELERATION - ); + ), + ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, + MAX_ANGLE_ACCELERATION + ); static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), @@ -72,18 +72,18 @@ public class ArmConstants { private static final double ANGLE_P = 0, ANGLE_I = 0, - ANGLE_D = 0; - static final PIDController ANGLE_PID_CONTROLLER = new PIDController( - ANGLE_P, - ANGLE_I, - ANGLE_D - ); - - private static final double + ANGLE_D = 0, ELEVATOR_P = 0, ELEVATOR_I = 0, ELEVATOR_D = 0; - static final PIDController ELEVATOR_PID_CONTROLLER = new PIDController( + + static final PIDController + ANGLE_PID_CONTROLLER = new PIDController( + ANGLE_P, + ANGLE_I, + ANGLE_D + ), + ELEVATOR_PID_CONTROLLER = new PIDController( ELEVATOR_P, ELEVATOR_I, ELEVATOR_D @@ -93,19 +93,18 @@ public class ArmConstants { ANGLE_MOTOR_KS = 0, ANGLE_MOTOR_KV = 0, ANGLE_MOTOR_KA = 0, - ANGLE_MOTOR_KG = 0; - static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KG = 0, + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ArmFeedforward + ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( ANGLE_MOTOR_KS, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA, ANGLE_MOTOR_KG ); - - private static final double - ELEVATOR_MOTOR_KS = 0, - ELEVATOR_MOTOR_KV = 0, - ELEVATOR_MOTOR_KA = 0, - ELEVATOR_MOTOR_KG = 0; static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward( ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, From f5dfefe0636a27d4fe161987bb267b7bade3cf75 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Sun, 24 Dec 2023 12:32:27 +0200 Subject: [PATCH 16/18] fixed according code review --- .../frc/trigon/robot/subsystems/arm/Arm.java | 98 +++++++++---------- .../robot/subsystems/arm/ArmCommands.java | 31 ++++-- 2 files changed, 70 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 4c85607a0..5820e560e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -1,6 +1,5 @@ package frc.trigon.robot.subsystems.arm; - import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Rotation2d; @@ -32,19 +31,19 @@ public static Arm getInstance() { private Arm() { } - void generateElevatorMotorProfile(double targetElevatorPositionMeters, double elevatorSpeedPercentage) { + void generateElevatorMotorProfile(double targetPositionMeters, double SpeedPercentage) { elevatorMotorProfile = new TrapezoidProfile( - Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, elevatorSpeedPercentage), - new TrapezoidProfile.State(targetElevatorPositionMeters, 0), + Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, SpeedPercentage), + new TrapezoidProfile.State(targetPositionMeters, 0), new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityMetersPerSeconds()) ); lastElevatorMotorProfileGeneration = Timer.getFPGATimestamp(); } - void generateAngleMotorProfile(Rotation2d targetAngle, double angleSpeedPercentage) { + void generateAngleMotorProfile(Rotation2d targetAngle, double SpeedPercentage) { angleMotorProfile = new TrapezoidProfile( - Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, angleSpeedPercentage), + Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, SpeedPercentage), new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSeconds()) ); @@ -52,28 +51,6 @@ void generateAngleMotorProfile(Rotation2d targetAngle, double angleSpeedPercenta lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } - private Rotation2d getAnglePosition() { - double positionRevolutions = ArmConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); - return Rotation2d.fromRotations(positionRevolutions); - } - - private double getElevatorPositionMeters() { - double elevatorPositionMagTicks = elevatorEncoder.getSelectedSensorPosition(); - double elevatorPositionRevolution = Conversions.magTicksToRevolutions(elevatorPositionMagTicks); - return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; - } - - private double getAngleVelocityDegreesPerSeconds() { - return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); - } - - private double getElevatorVelocityMetersPerSeconds(){ - double elevatorVelocityMagTicksPer100ms = elevatorEncoder.getSelectedSensorVelocity(); - double elevatorVelocityMagTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorVelocityMagTicksPer100ms); - double elevatorVelocityRevolutionPerSecond = Conversions.magTicksToRevolutions(elevatorVelocityMagTicksPerSecond); - return elevatorVelocityRevolutionPerSecond / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; - } - void setTargetAngleFromProfile() { if (angleMotorProfile == null) { masterAngleMotor.stopMotor(); @@ -96,37 +73,54 @@ void setTargetElevatorPositionFromProfile() { setElevatorMotorsVoltage(calculateElevatorOutput(targetState)); } - private double calculateElevatorOutput(TrapezoidProfile.State targetState){ - double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getAnglePosition().getDegrees(), - targetState.position - ); + boolean atAngle(Rotation2d angle) { + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE_DEGREES; + } - double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( - targetState.velocity - ); + boolean atElevatorMeters(double PositionMeters) { + return Math.abs(PositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_DEGREES; + } - return pidOutput + feedforward; + boolean isElevatorOpening(double targetMeters) { + return targetMeters > getElevatorPositionMeters(); } - private void setAngleMotorsVoltage(double outputVoltage){ - masterAngleMotor.setVoltage(outputVoltage); - followerAngleMotor.setVoltage(outputVoltage); + private double getElevatorVelocityMetersPerSeconds() { + double elevatorVelocityMagTicksPer100ms = elevatorEncoder.getSelectedSensorVelocity(); + double elevatorVelocityMagTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorVelocityMagTicksPer100ms); + double elevatorVelocityRevolutionPerSecond = Conversions.magTicksToRevolutions(elevatorVelocityMagTicksPerSecond); + return elevatorVelocityRevolutionPerSecond / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; + } + + private Rotation2d getAnglePosition() { + double positionRevolutions = ArmConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); } - boolean atAngle(Rotation2d angle){ - return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE_DEGREES; + private double getElevatorPositionMeters() { + double elevatorPositionMagTicks = elevatorEncoder.getSelectedSensorPosition(); + double elevatorPositionRevolution = Conversions.magTicksToRevolutions(elevatorPositionMagTicks); + return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; } - boolean atElevatorMeters(double ElevatorPositionMeters){ - return Math.abs(ElevatorPositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_DEGREES; + private double getAngleVelocityDegreesPerSeconds() { + return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } - boolean isElevatorOpening(double targetElevatorMeters){ - return targetElevatorMeters > getElevatorPositionMeters(); + private double calculateElevatorOutput(TrapezoidProfile.State targetState) { + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( + targetState.velocity + ); + + return pidOutput + feedforward; } - private double calculateAngleOutput(TrapezoidProfile.State targetState){ + private double calculateAngleOutput(TrapezoidProfile.State targetState) { double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( getAnglePosition().getDegrees(), targetState.position @@ -140,7 +134,13 @@ private double calculateAngleOutput(TrapezoidProfile.State targetState){ return pidOutput + feedforward; } - private void setElevatorMotorsVoltage(double outputVoltage){ + + private void setAngleMotorsVoltage(double outputVoltage) { + masterAngleMotor.setVoltage(outputVoltage); + followerAngleMotor.setVoltage(outputVoltage); + } + + private void setElevatorMotorsVoltage(double outputVoltage) { masterElevatorMotor.setVoltage(outputVoltage); followerElevatorMotor.setVoltage(outputVoltage); } @@ -149,7 +149,7 @@ private double getAngleProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; } - private double getElevatorProfileTime(){ + private double getElevatorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorMotorProfileGeneration; } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 94ac885b8..7f538ede0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -2,36 +2,47 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import java.util.Set; + public class ArmCommands { private static final Arm ARM = Arm.getInstance(); - public static Command getSetTargetStateCommand(ArmConstants.ArmState state){ + public static Command getSetTargetStateCommand(ArmConstants.ArmState state) { return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, 100, 100); } - public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentages, double angleSpeedPercentages){ - return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters,elevatorSpeedPercentages, angleSpeedPercentages); + public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentages, double angleSpeedPercentages) { + return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, elevatorSpeedPercentages, angleSpeedPercentages); + } + + public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters) { + return getSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, 100, 100); + } + + public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentages) { + return new DeferredCommand(() -> getSetCurrentTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentages, angleSpeedPercentages), Set.of()); } - public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentage) { + public static Command getSetCurrentTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentage) { if (ARM.isElevatorOpening(targetElevatorPositionMeters)) { return new SequentialCommandGroup( getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)), - getSetTargetElevatorPositionMetersCommand(targetElevatorPositionMeters,elevatorSpeedPercentages) + getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentages) ); } return new SequentialCommandGroup( - getSetTargetElevatorPositionMetersCommand(targetElevatorPositionMeters, elevatorSpeedPercentages).until(()-> ARM.atElevatorMeters(targetElevatorPositionMeters)), + getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentages).until(() -> ARM.atElevatorMeters(targetElevatorPositionMeters)), getSetTargetAngleCommand(targetAngle, angleSpeedPercentage) ); } - public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double angleSpeedPercentage) { + public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double SpeedPercentage) { return new FunctionalCommand( - () -> ARM.generateAngleMotorProfile(targetAngle, angleSpeedPercentage), + () -> ARM.generateAngleMotorProfile(targetAngle, SpeedPercentage), ARM::setTargetAngleFromProfile, (interrupted) -> { }, @@ -40,9 +51,9 @@ public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double an ); } - public static Command getSetTargetElevatorPositionMetersCommand(double targetElevatorPositionMeters, double elevatorSpeedPercentage) { + public static Command getSetTargetElevatorPositionCommand(double targetElevatorPositionMeters, double SpeedPercentage) { return new FunctionalCommand( - () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, elevatorSpeedPercentage), + () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, SpeedPercentage), ARM::setTargetElevatorPositionFromProfile, (interrupted) -> { }, From 6adcef767ce9ec38dd2c71875b1e32b29cfcbba7 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 1 Jan 2024 16:21:50 +0200 Subject: [PATCH 17/18] fixed according code review --- .../frc/trigon/robot/subsystems/arm/Arm.java | 22 +++++++++++++------ .../robot/subsystems/arm/ArmCommands.java | 14 ++++++------ .../robot/subsystems/arm/ArmConstants.java | 14 ++++++------ 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 5820e560e..6e4acdbb5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -13,9 +13,9 @@ public class Arm extends SubsystemBase { private final static Arm INSTANCE = new Arm(); private final CANSparkMax masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, - followerElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR, masterAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, - followerAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR; + followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR; private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; private TrapezoidProfile angleMotorProfile = null, @@ -53,8 +53,7 @@ void generateAngleMotorProfile(Rotation2d targetAngle, double SpeedPercentage) { void setTargetAngleFromProfile() { if (angleMotorProfile == null) { - masterAngleMotor.stopMotor(); - followerAngleMotor.stopMotor(); + stopAngleMotors(); return; } @@ -64,8 +63,7 @@ void setTargetAngleFromProfile() { void setTargetElevatorPositionFromProfile() { if (angleMotorProfile == null) { - masterElevatorMotor.stopMotor(); - followerElevatorMotor.stopMotor(); + stopElevatorMotors(); return; } @@ -78,7 +76,7 @@ boolean atAngle(Rotation2d angle) { } boolean atElevatorMeters(double PositionMeters) { - return Math.abs(PositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_DEGREES; + return Math.abs(PositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_METERS; } boolean isElevatorOpening(double targetMeters) { @@ -152,4 +150,14 @@ private double getAngleProfileTime() { private double getElevatorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorMotorProfileGeneration; } + + private void stopElevatorMotors() { + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); + } + + private void stopAngleMotors() { + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 7f538ede0..b049223b2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -15,27 +15,27 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState state) { return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, 100, 100); } - public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentages, double angleSpeedPercentages) { - return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, elevatorSpeedPercentages, angleSpeedPercentages); + public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentage, double angleSpeedPercentage) { + return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage); } public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters) { return getSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, 100, 100); } - public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentages) { - return new DeferredCommand(() -> getSetCurrentTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentages, angleSpeedPercentages), Set.of()); + public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) { + return new DeferredCommand(() -> getSetCurrentTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage), Set.of()); } - public static Command getSetCurrentTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentages, double angleSpeedPercentage) { + public static Command getSetCurrentTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) { if (ARM.isElevatorOpening(targetElevatorPositionMeters)) { return new SequentialCommandGroup( getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)), - getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentages) + getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage) ); } return new SequentialCommandGroup( - getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentages).until(() -> ARM.atElevatorMeters(targetElevatorPositionMeters)), + getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage).until(() -> ARM.atElevatorMeters(targetElevatorPositionMeters)), getSetTargetAngleCommand(targetAngle, angleSpeedPercentage) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 235e27237..2baca0963 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -41,15 +41,15 @@ public class ArmConstants { private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final CANSparkMax MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), - FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), - FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); static final double ELEVATOR_METERS_PER_REVOLUTION = 1; static final double ANGLE_TOLERANCE_DEGREES = 1, - ELEVATOR_TOLERANCE_DEGREES = 1; + ELEVATOR_TOLERANCE_METERS = 1; private static final double MAX_ELEVATOR_VELOCITY = 0, @@ -84,10 +84,10 @@ public class ArmConstants { ANGLE_D ), ELEVATOR_PID_CONTROLLER = new PIDController( - ELEVATOR_P, - ELEVATOR_I, - ELEVATOR_D - ); + ELEVATOR_P, + ELEVATOR_I, + ELEVATOR_D + ); private static final double ANGLE_MOTOR_KS = 0, From 55212d1b9887c7d802e96fcee7f83d7ef840d042 Mon Sep 17 00:00:00 2001 From: yishay23 Date: Mon, 1 Jan 2024 16:55:23 +0200 Subject: [PATCH 18/18] fixed according code review --- .../frc/trigon/robot/subsystems/arm/Arm.java | 21 +++++++-------- .../robot/subsystems/arm/ArmCommands.java | 27 +++++++++++++++---- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../subsystems/sideshooter/SideShooter.java | 8 +++--- 4 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 6e4acdbb5..4285aea05 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -31,19 +31,19 @@ public static Arm getInstance() { private Arm() { } - void generateElevatorMotorProfile(double targetPositionMeters, double SpeedPercentage) { + void generateElevatorMotorProfile(double targetPositionMeters, double speedPercentage) { elevatorMotorProfile = new TrapezoidProfile( - Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, SpeedPercentage), + Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, speedPercentage), new TrapezoidProfile.State(targetPositionMeters, 0), - new TrapezoidProfile.State(getElevatorPositionMeters(), getElevatorVelocityMetersPerSeconds()) + new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityMetersPerSeconds()) ); lastElevatorMotorProfileGeneration = Timer.getFPGATimestamp(); } - void generateAngleMotorProfile(Rotation2d targetAngle, double SpeedPercentage) { + void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) { angleMotorProfile = new TrapezoidProfile( - Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, SpeedPercentage), + Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, speedPercentage), new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSeconds()) ); @@ -75,12 +75,12 @@ boolean atAngle(Rotation2d angle) { return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE_DEGREES; } - boolean atElevatorMeters(double PositionMeters) { - return Math.abs(PositionMeters - getElevatorPositionMeters()) < ArmConstants.ELEVATOR_TOLERANCE_METERS; + boolean atTargetPositionElevator(double positionMeters) { + return Math.abs(positionMeters - getElevatorPositionRevolutions()) < ArmConstants.ELEVATOR_TOLERANCE_METERS; } boolean isElevatorOpening(double targetMeters) { - return targetMeters > getElevatorPositionMeters(); + return targetMeters > getElevatorPositionRevolutions(); } private double getElevatorVelocityMetersPerSeconds() { @@ -95,7 +95,7 @@ private Rotation2d getAnglePosition() { return Rotation2d.fromRotations(positionRevolutions); } - private double getElevatorPositionMeters() { + private double getElevatorPositionRevolutions() { double elevatorPositionMagTicks = elevatorEncoder.getSelectedSensorPosition(); double elevatorPositionRevolution = Conversions.magTicksToRevolutions(elevatorPositionMagTicks); return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION; @@ -107,7 +107,7 @@ private double getAngleVelocityDegreesPerSeconds() { private double calculateElevatorOutput(TrapezoidProfile.State targetState) { double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getAnglePosition().getDegrees(), + getElevatorPositionRevolutions(), targetState.position ); @@ -132,7 +132,6 @@ private double calculateAngleOutput(TrapezoidProfile.State targetState) { return pidOutput + feedforward; } - private void setAngleMotorsVoltage(double outputVoltage) { masterAngleMotor.setVoltage(outputVoltage); followerAngleMotor.setVoltage(outputVoltage); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index b049223b2..ed659c0ab 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -15,6 +15,14 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState state) { return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, 100, 100); } + /** + * set the arm to the wanted state + * + * @param state the wanted state + * @param elevatorSpeedPercentage max speed of the elevator in percentages + * @param angleSpeedPercentage max speed of the elevator angle in percentages + * @return a command + */ public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentage, double angleSpeedPercentage) { return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage); } @@ -23,11 +31,20 @@ public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double return getSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, 100, 100); } + /** + * set the arm to the wanted position + * + * @param targetAngle the target angle + * @param targetElevatorPositionMeters the target of the elevator + * @param elevatorSpeedPercentage max speed of the elevator in percentages + * @param angleSpeedPercentage max speed of the elevator angle in percentages + * @return a command + */ public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) { - return new DeferredCommand(() -> getSetCurrentTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage), Set.of()); + return new DeferredCommand(() -> getCurrentSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage), Set.of()); } - public static Command getSetCurrentTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) { + private static Command getCurrentSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) { if (ARM.isElevatorOpening(targetElevatorPositionMeters)) { return new SequentialCommandGroup( getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)), @@ -35,12 +52,12 @@ public static Command getSetCurrentTargetPositionCommand(Rotation2d targetAngle, ); } return new SequentialCommandGroup( - getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage).until(() -> ARM.atElevatorMeters(targetElevatorPositionMeters)), + getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage).until(() -> ARM.atTargetPositionElevator(targetElevatorPositionMeters)), getSetTargetAngleCommand(targetAngle, angleSpeedPercentage) ); } - public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double SpeedPercentage) { + private static Command getSetTargetAngleCommand(Rotation2d targetAngle, double SpeedPercentage) { return new FunctionalCommand( () -> ARM.generateAngleMotorProfile(targetAngle, SpeedPercentage), ARM::setTargetAngleFromProfile, @@ -51,7 +68,7 @@ public static Command getSetTargetAngleCommand(Rotation2d targetAngle, double Sp ); } - public static Command getSetTargetElevatorPositionCommand(double targetElevatorPositionMeters, double SpeedPercentage) { + private static Command getSetTargetElevatorPositionCommand(double targetElevatorPositionMeters, double SpeedPercentage) { return new FunctionalCommand( () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, SpeedPercentage), ARM::setTargetElevatorPositionFromProfile, diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 2baca0963..d9e757a45 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -46,6 +46,7 @@ public class ArmConstants { FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); + static final double ELEVATOR_METERS_PER_REVOLUTION = 1; static final double ANGLE_TOLERANCE_DEGREES = 1, @@ -76,7 +77,6 @@ public class ArmConstants { ELEVATOR_P = 0, ELEVATOR_I = 0, ELEVATOR_D = 0; - static final PIDController ANGLE_PID_CONTROLLER = new PIDController( ANGLE_P, diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java index d890dafaf..d7045d1c0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -15,7 +15,7 @@ public class SideShooter extends SubsystemBase { private final static SideShooter INSTANCE = new SideShooter(); private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; - private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); + private final VoltageOut shootingVoltageRequest = new VoltageOut(0).withEnableFOC(SideShooterConstants.FOC_ENABLED); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGeneration; @@ -54,11 +54,11 @@ void stopShooting() { shootingMotor.stopMotor(); } - boolean atAngle(Rotation2d angle){ - return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.ANGLE_TOLERANCE; + boolean atAngle(Rotation2d angle) { + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.ANGLE_TOLERANCE; } - private double calculateAngleOutput(TrapezoidProfile.State targetState){ + private double calculateAngleOutput(TrapezoidProfile.State targetState) { double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( getAnglePosition().getDegrees(), targetState.position