From 5e0694a3452a70e37e5739e60b5025891efef3be Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 12 Nov 2023 18:08:00 +0200 Subject: [PATCH 01/19] 90% of the constans --- .../SideShooter/SideShooterConstants.java | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 src/main/java/SideShooter/SideShooterConstants.java diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java new file mode 100644 index 000000000..8774d9760 --- /dev/null +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -0,0 +1,66 @@ +package 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.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import com.revrobotics.SparkMaxPIDController; +import edu.wpi.first.math.controller.PIDController; + +import java.time.OffsetDateTime; +import java.time.OffsetTime; + +public class SideShooterConstants { + + + private static final int MOTOR_SHOOT_ID = 0; + private static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final int MOTOR_angel_ID = 0; + private static SparkMaxPIDController MOTOR_angel_PIDController; + static final CANSparkMax MOTOR_angel = new CANSparkMax(MOTOR_angel_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + private final static int Encoder_ID = 0; + private static final CANcoder encoder = new CANcoder(Encoder_ID); + + private static final int + P= 0, + I = 0, + D = 0; + + static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + + static final TalonFX MOTOR_SHOOT = new TalonFX(MOTOR_SHOOT_ID); + + + static { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.MotorOutput.Inverted = INVERTED_VALUE; + config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; + config.Audio.BeepOnConfig = false; + + MOTOR_angel.restoreFactoryDefaults(); + MOTOR_angel_PIDController = MOTOR_angel.getPIDController(); + MOTOR_angel.setInverted(false); + MOTOR_angel.setIdleMode(CANSparkMax.IdleMode.kBrake); + MOTOR_angel.setVoltage(12); + + MOTOR_SHOOT.getConfigurator().apply(config); + CANcoderConfiguration configuration = new CANcoderConfiguration(); + configuration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + configuration.MagnetSensor.MagnetOffset = 0; + configuration.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + + } + + + + + +} From 393d100b8a4e4e7afa943ee9e14525e0bbc0eeab Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 18:07:10 +0200 Subject: [PATCH 02/19] make side shooter constans --- .../SideShooter/SideShooterConstants.java | 89 ++++++++++++------- 1 file changed, 56 insertions(+), 33 deletions(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 8774d9760..7fcd62543 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -10,57 +10,80 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; -import com.revrobotics.SparkMaxPIDController; import edu.wpi.first.math.controller.PIDController; - -import java.time.OffsetDateTime; -import java.time.OffsetTime; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; public class SideShooterConstants { - - - private static final int MOTOR_SHOOT_ID = 0; - private static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final int SHOOTING_MOTOR_ID = 0; + private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; - private static final int MOTOR_angel_ID = 0; - private static SparkMaxPIDController MOTOR_angel_PIDController; - static final CANSparkMax MOTOR_angel = new CANSparkMax(MOTOR_angel_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final int ANGLE_MOTOR_ID = 0; + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); private final static int Encoder_ID = 0; - private static final CANcoder encoder = new CANcoder(Encoder_ID); + private static final CANcoder ANGLE_ENCODER = new CANcoder(Encoder_ID); - private static final int - P= 0, - I = 0, - D = 0; + private static final double + MAX_ANGEL_VELOCITY = 600, - static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + MAX_ANGEL_ACCELERATION = 500; - static final TalonFX MOTOR_SHOOT = new TalonFX(MOTOR_SHOOT_ID); + private static final int + ANGLE_MOTOR_P = 0, + ANGLE_MOTOR_I = 0, + ANGLE_MOTOR_D = 0; - static { + static final PIDController ANGLE_PID_CONTROLLER = new PIDController(ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D); + + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + + private static void configureShootingMotor() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Audio.BeepOnBoot = false; - config.MotorOutput.Inverted = INVERTED_VALUE; + config.Audio.BeepOnBoot = true; + config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; config.Audio.BeepOnConfig = false; + SHOOTING_MOTOR.getConfigurator().apply(config); + } - MOTOR_angel.restoreFactoryDefaults(); - MOTOR_angel_PIDController = MOTOR_angel.getPIDController(); - MOTOR_angel.setInverted(false); - MOTOR_angel.setIdleMode(CANSparkMax.IdleMode.kBrake); - MOTOR_angel.setVoltage(12); - - MOTOR_SHOOT.getConfigurator().apply(config); - CANcoderConfiguration configuration = new CANcoderConfiguration(); - configuration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - configuration.MagnetSensor.MagnetOffset = 0; - configuration.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + private static void configureAngleMotor() { + ANGLE_MOTOR.restoreFactoryDefaults(); + ANGLE_PID_CONTROLLER.setSetpoint(0); + ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); + ANGLE_MOTOR.setIdleMode(CANSparkMax.IdleMode.kBrake); + ANGLE_MOTOR.enableVoltageCompensation(12); + } + private static void configureAngleEncoder() { + CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); + configureAngleMotor.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + configureAngleMotor.MagnetSensor.MagnetOffset = 0; + configureAngleMotor.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } + static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( + MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION + ); + static { + configureAngleEncoder(); + configureAngleMotor(); + configureShootingMotor(); + } + + public enum SideShooterState { + COLLECT_POSITION(Rotation2d.fromDegrees(0)), + MID_LEVEL_POSITION(Rotation2d.fromDegrees(222)), + HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3)); + private Rotation2d angel; + SideShooterState(Rotation2d angel){ + this.angel = angel; -} + } + } +} \ No newline at end of file From 5fbe4a6bbf61bb6c7abb558b438356d139b79f44 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 18:57:53 +0200 Subject: [PATCH 03/19] control+alt+L --- .../SideShooter/SideShooterConstants.java | 47 ++++++++++--------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 7fcd62543..814e188ad 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -1,5 +1,6 @@ package SideShooter; +import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; @@ -16,19 +17,26 @@ public class SideShooterConstants { private static final int SHOOTING_MOTOR_ID = 0; + private static final int ANGLE_MOTOR_ID = 0; + private static final int ENCODER_ID = 0; + private static final double MAGNET_OFFSET = 0; + private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final AbsoluteSensorRangeValue ANGEL_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final double ANGLE_MOTOR_ENABLE_VOLTAGE_COMPENEATION = 16.45; private static final boolean ANGLE_MOTOR_INVERTED = false; - private static final int ANGLE_MOTOR_ID = 0; + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); - private final static int Encoder_ID = 0; - private static final CANcoder ANGLE_ENCODER = new CANcoder(Encoder_ID); + private static final CANcoder ANGLE_ENCODER = new CANcoder(ENCODER_ID); private static final double MAX_ANGEL_VELOCITY = 600, - MAX_ANGEL_ACCELERATION = 500; - + static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( + MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION + ); private static final int ANGLE_MOTOR_P = 0, @@ -37,7 +45,11 @@ public class SideShooterConstants { static final PIDController ANGLE_PID_CONTROLLER = new PIDController(ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D); - static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + static { + configureAngleEncoder(); + configureAngleMotor(); + configureShootingMotor(); + } private static void configureShootingMotor() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -50,37 +62,26 @@ private static void configureShootingMotor() { private static void configureAngleMotor() { ANGLE_MOTOR.restoreFactoryDefaults(); - ANGLE_PID_CONTROLLER.setSetpoint(0); ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); - ANGLE_MOTOR.setIdleMode(CANSparkMax.IdleMode.kBrake); - ANGLE_MOTOR.enableVoltageCompensation(12); + ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); + ANGLE_MOTOR.enableVoltageCompensation(ANGLE_MOTOR_ENABLE_VOLTAGE_COMPENEATION); } private static void configureAngleEncoder() { CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); - configureAngleMotor.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - configureAngleMotor.MagnetSensor.MagnetOffset = 0; - configureAngleMotor.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; + configureAngleMotor.MagnetSensor.MagnetOffset = MAGNET_OFFSET; + configureAngleMotor.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } - static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( - MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION - ); - - static { - configureAngleEncoder(); - configureAngleMotor(); - configureShootingMotor(); - } - public enum SideShooterState { COLLECT_POSITION(Rotation2d.fromDegrees(0)), MID_LEVEL_POSITION(Rotation2d.fromDegrees(222)), HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3)); private Rotation2d angel; - SideShooterState(Rotation2d angel){ + SideShooterState(Rotation2d angel) { this.angel = angel; From fba1a7f9074970d89642241ca49bde3f4c5dece5 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:30:36 +0200 Subject: [PATCH 04/19] rename --- .../java/SideShooter/SideShooterConstants.java | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 814e188ad..b83d763d4 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -1,6 +1,5 @@ package SideShooter; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; @@ -19,13 +18,13 @@ public class SideShooterConstants { private static final int SHOOTING_MOTOR_ID = 0; private static final int ANGLE_MOTOR_ID = 0; private static final int ENCODER_ID = 0; - private static final double MAGNET_OFFSET = 0; + private static final double ANGLE_MAGNET_OFFSET = 0; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGEL_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; - private static final double ANGLE_MOTOR_ENABLE_VOLTAGE_COMPENEATION = 16.45; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final double VOLTAGE_COMPENSATION_SATURATION = 16.45; private static final boolean ANGLE_MOTOR_INVERTED = false; static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); @@ -55,7 +54,7 @@ private static void configureShootingMotor() { TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = true; config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; - config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; + config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; config.Audio.BeepOnConfig = false; SHOOTING_MOTOR.getConfigurator().apply(config); } @@ -64,14 +63,14 @@ private static void configureAngleMotor() { ANGLE_MOTOR.restoreFactoryDefaults(); ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); - ANGLE_MOTOR.enableVoltageCompensation(ANGLE_MOTOR_ENABLE_VOLTAGE_COMPENEATION); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); } private static void configureAngleEncoder() { CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; - configureAngleMotor.MagnetSensor.MagnetOffset = MAGNET_OFFSET; - configureAngleMotor.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION; + configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_MAGNET_OFFSET; + configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } From da85522f925c94b55761bf1cf249da007693eaaf Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:35:23 +0200 Subject: [PATCH 05/19] false --- src/main/java/SideShooter/SideShooterConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index b83d763d4..fc3f04456 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -52,7 +52,7 @@ public class SideShooterConstants { private static void configureShootingMotor() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Audio.BeepOnBoot = true; + config.Audio.BeepOnBoot = false; config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; config.Audio.BeepOnConfig = false; From 35f9790161b2189e8e8ac7ec03355e70859612e0 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:53:46 +0200 Subject: [PATCH 06/19] stopid think --- src/main/java/SideShooter/SideShooterConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index fc3f04456..5adee071e 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -18,13 +18,13 @@ public class SideShooterConstants { private static final int SHOOTING_MOTOR_ID = 0; private static final int ANGLE_MOTOR_ID = 0; private static final int ENCODER_ID = 0; - private static final double ANGLE_MAGNET_OFFSET = 0; + private static final double ANGLE_ENCODER_OFFSET = 0; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGEL_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; - private static final double VOLTAGE_COMPENSATION_SATURATION = 16.45; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final boolean ANGLE_MOTOR_INVERTED = false; static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); @@ -69,7 +69,7 @@ private static void configureAngleMotor() { private static void configureAngleEncoder() { CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; - configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_MAGNET_OFFSET; + configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } @@ -78,10 +78,10 @@ public enum SideShooterState { COLLECT_POSITION(Rotation2d.fromDegrees(0)), MID_LEVEL_POSITION(Rotation2d.fromDegrees(222)), HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3)); - private Rotation2d angel; + private Rotation2d angle; SideShooterState(Rotation2d angel) { - this.angel = angel; + this.angle = angel; } From 9eebd787740c9c774c077ab62d7586e1f73afe90 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:57:14 +0200 Subject: [PATCH 07/19] =?UTF-8?q?=D7=AA=D7=A2=D7=A9=D7=95=20=D7=90=D7=AA?= =?UTF-8?q?=20=D7=96=D7=94=20=D7=9B=D7=90=D7=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/SideShooter/SideShooterConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 5adee071e..4b45b9a2e 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -37,7 +37,7 @@ public class SideShooterConstants { MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION ); - private static final int + private static final double ANGLE_MOTOR_P = 0, ANGLE_MOTOR_I = 0, ANGLE_MOTOR_D = 0; From 8bb0443da3b02b9a776d37c4d8ed2be2bba26586 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:00:16 +0200 Subject: [PATCH 08/19] sent all the problams --- src/main/java/SideShooter/SideShooterConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 4b45b9a2e..7b8856a5a 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -23,7 +23,7 @@ public class SideShooterConstants { private static final AbsoluteSensorRangeValue ANGEL_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final boolean ANGLE_MOTOR_INVERTED = false; static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); From dfb02234a84392a41cda56b065753c0aa550f386 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:11:13 +0200 Subject: [PATCH 09/19] =?UTF-8?q?=D7=94=D7=95=D7=A1=D7=A4=D7=AA=D7=99=20?= =?UTF-8?q?=D7=A4=D7=99=D7=93=D7=A4=D7=95=D7=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/SideShooter/SideShooterConstants.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/SideShooter/SideShooterConstants.java index 7b8856a5a..6d07ecc7c 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/SideShooter/SideShooterConstants.java @@ -10,6 +10,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; @@ -36,13 +37,24 @@ public class SideShooterConstants { static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION ); + static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_ks, ANGLE_MOTOR_kg, ANGLE_MOTOR_kv, ANGLE_MOTOR_ka + ); private static final double ANGLE_MOTOR_P = 0, ANGLE_MOTOR_I = 0, ANGLE_MOTOR_D = 0; - static final PIDController ANGLE_PID_CONTROLLER = new PIDController(ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D); + private static final double + ANGLE_MOTOR_ks = 0.58835, + ANGLE_MOTOR_kv = 0.74627, + ANGLE_MOTOR_ka = 0.37502, + ANGLE_MOTOR_kg = 0.92056; + + static final PIDController ANGLE_PID_CONTROLLER = new PIDController( + ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D + ); static { configureAngleEncoder(); From 9d1f535001bd7c5cb1cc4d5f88915e77f0ea0edb Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:33:14 +0200 Subject: [PATCH 10/19] =?UTF-8?q?=D7=94=D7=A2=D7=91=D7=A8=D7=AA=D7=99=20?= =?UTF-8?q?=D7=AA=D7=A7=D7=99=D7=99=D7=94?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sideshooter}/SideShooterConstants.java | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) rename src/main/java/{SideShooter => frc/trigon/robot/subsystems/sideshooter}/SideShooterConstants.java (92%) diff --git a/src/main/java/SideShooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java similarity index 92% rename from src/main/java/SideShooter/SideShooterConstants.java rename to src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java index 6d07ecc7c..9f6565962 100644 --- a/src/main/java/SideShooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -1,4 +1,4 @@ -package SideShooter; +package frc.trigon.robot.subsystems.sideshooter; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -37,9 +37,6 @@ public class SideShooterConstants { static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION ); - static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( - ANGLE_MOTOR_ks, ANGLE_MOTOR_kg, ANGLE_MOTOR_kv, ANGLE_MOTOR_ka - ); private static final double ANGLE_MOTOR_P = 0, @@ -52,6 +49,10 @@ public class SideShooterConstants { ANGLE_MOTOR_ka = 0.37502, ANGLE_MOTOR_kg = 0.92056; + static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_ks, ANGLE_MOTOR_kg, ANGLE_MOTOR_kv, ANGLE_MOTOR_ka + ); + static final PIDController ANGLE_PID_CONTROLLER = new PIDController( ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D ); @@ -87,13 +88,15 @@ private static void configureAngleEncoder() { } public enum SideShooterState { - COLLECT_POSITION(Rotation2d.fromDegrees(0)), - MID_LEVEL_POSITION(Rotation2d.fromDegrees(222)), - HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3)); + COLLECT_POSITION(Rotation2d.fromDegrees(0),0), + MID_LEVEL_POSITION(Rotation2d.fromDegrees(222), 9), + HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3), 78); private Rotation2d angle; + private double voltage; - SideShooterState(Rotation2d angel) { + SideShooterState(Rotation2d angel, double voltage) { this.angle = angel; + this.voltage = voltage; } From c36928f539b132bada9fdbbd7ca4992d02c021a6 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:45:44 +0200 Subject: [PATCH 11/19] last constans revyu --- .../robot/subsystems/sideshooter/SideShooterConstants.java | 6 ++++++ 1 file changed, 6 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 9f6565962..1cd50acbd 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; @@ -57,6 +58,8 @@ public class SideShooterConstants { ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D ); + static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static { configureAngleEncoder(); configureAngleMotor(); @@ -69,6 +72,7 @@ private static void configureShootingMotor() { config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; config.Audio.BeepOnConfig = false; + SHOOTING_MOTOR.optimizeBusUtilization(); SHOOTING_MOTOR.getConfigurator().apply(config); } @@ -84,6 +88,8 @@ private static void configureAngleEncoder() { configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } From 6ae9dc72c6cee52de45cba9e95e19656a14c0af2 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:52:34 +0200 Subject: [PATCH 12/19] final --- .../sideshooter/SideShooterConstants.java | 33 +++++++++---------- 1 file changed, 15 insertions(+), 18 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 1cd50acbd..9d94f0a15 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -43,19 +43,17 @@ public class SideShooterConstants { ANGLE_MOTOR_P = 0, ANGLE_MOTOR_I = 0, ANGLE_MOTOR_D = 0; + static final PIDController ANGLE_PID_CONTROLLER = new PIDController( + ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D + ); private static final double - ANGLE_MOTOR_ks = 0.58835, - ANGLE_MOTOR_kv = 0.74627, - ANGLE_MOTOR_ka = 0.37502, - ANGLE_MOTOR_kg = 0.92056; - + ANGLE_MOTOR_KS = 0.58835, + ANGLE_MOTOR_KV = 0.74627, + ANGLE_MOTOR_KA = 0.37502, + ANGLE_MOTOR_KG = 0.92056; static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( - ANGLE_MOTOR_ks, ANGLE_MOTOR_kg, ANGLE_MOTOR_kv, ANGLE_MOTOR_ka - ); - - static final PIDController ANGLE_PID_CONTROLLER = new PIDController( - ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); @@ -69,9 +67,9 @@ public class SideShooterConstants { private static void configureShootingMotor() { TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; - config.Audio.BeepOnConfig = false; SHOOTING_MOTOR.optimizeBusUtilization(); SHOOTING_MOTOR.getConfigurator().apply(config); } @@ -94,17 +92,16 @@ private static void configureAngleEncoder() { } public enum SideShooterState { - COLLECT_POSITION(Rotation2d.fromDegrees(0),0), + COLLECT_POSITION(Rotation2d.fromDegrees(0), 0), MID_LEVEL_POSITION(Rotation2d.fromDegrees(222), 9), HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3), 78); - private Rotation2d angle; - private double voltage; - - SideShooterState(Rotation2d angel, double voltage) { - this.angle = angel; - this.voltage = voltage; + Rotation2d angle; + double shootingVoltage; + SideShooterState(Rotation2d angel, double shootingVoltage) { + this.angle = angel; + this.shootingVoltage = shootingVoltage; } } } \ No newline at end of file From e46b8e176a9bea500f28f870ae2347846e46ace1 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:59:37 +0200 Subject: [PATCH 13/19] #$%^&^%$%^&*&^%$#$%^&*&^%$%^&*&^% --- .../subsystems/sideshooter/SideShooterConstants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 9d94f0a15..70def970c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -70,8 +70,8 @@ private static void configureShootingMotor() { config.Audio.BeepOnConfig = false; config.MotorOutput.Inverted = SHOOTER_INVERTED_VALUE; config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; - SHOOTING_MOTOR.optimizeBusUtilization(); SHOOTING_MOTOR.getConfigurator().apply(config); + SHOOTING_MOTOR.optimizeBusUtilization(); } private static void configureAngleMotor() { @@ -86,8 +86,8 @@ private static void configureAngleEncoder() { configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; - ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); } @@ -96,8 +96,8 @@ public enum SideShooterState { MID_LEVEL_POSITION(Rotation2d.fromDegrees(222), 9), HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3), 78); - Rotation2d angle; - double shootingVoltage; + final Rotation2d angle; + final double shootingVoltage; SideShooterState(Rotation2d angel, double shootingVoltage) { this.angle = angel; From 8b2c101fbc237c91be4ba37fbae189fd8f49f0a4 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Tue, 5 Dec 2023 18:47:27 +0200 Subject: [PATCH 14/19] =?UTF-8?q?=D7=A9=D7=9D=20=D7=A9=D7=A0=D7=95=D7=A2?= =?UTF-8?q?=D7=93=20=D7=9C=D7=A2=D7=A9=D7=95=D7=AA=20=D7=9C=D7=99=D7=A9?= =?UTF-8?q?=D7=99=20=D7=91=D7=95=D7=A9=D7=95=D7=AA=20=D7=9E=D7=95=D7=9C=20?= =?UTF-8?q?=D7=A4=D7=99=D7=99=D7=A8=D7=A4=D7=9C=D7=99=D7=99=D7=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../subsystems/sideshooter/SideShooter.java | 275 ++++++++++++++++++ .../sideshooter/SideShooterConstants.java | 9 +- 2 files changed, 281 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.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..61187452a --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -0,0 +1,275 @@ +package frc.trigon.robot.subsystems.sideshooter; + + +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.controller.ArmFeedforward; +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.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.lang.annotation.Target; + +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; + + public static SideShooter getInstance() { + return INSTANCE; + } + + private Rotation2d getAngleMotorPoison() { + double positionRevolution = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolution); + } + + private Double getAngleMotorVelocity() { + return SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.getValue(); + } + + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + + 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(getAngleMotorPoison().getDegrees(), getAngleMotorVelocity())); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + } + + private void setTargetAngleMotorProfileTime() { + if (angleMotorProfile == null) { + angleMotor.stopMotor(); + return; + } + + TrapezoidProfile.State targetGetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleMotorPoison().getDegrees(), + targetGetState.position + ); + double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( + Units.degreesToRadians(targetGetState.position), + Units.degreesToRadians(targetGetState.velocity), + targetGetState.position + ); + + + angleMotor.setVoltage(pidOutPut = Feedforward); + } + public Command getSetTargetShooterAngleCommand(Rotation2d angle) { + return new FunctionalCommand( + () -> generateAngleMotorProfile(angle), + this::setTargetAngleMotorProfileTime, + (interrupted) -> {}, + () -> false, + this + ); + } +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 70def970c..2aebbb6b2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -56,7 +56,9 @@ public class SideShooterConstants { ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); - static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static final StatusSignal + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static final StatusSignal ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); static { configureAngleEncoder(); @@ -86,9 +88,10 @@ private static void configureAngleEncoder() { configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; - ANGLE_ENCODER.optimizeBusUtilization(); - ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); } public enum SideShooterState { From f427577578f551f8032d526ed893aa1463252c07 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Tue, 5 Dec 2023 19:56:12 +0200 Subject: [PATCH 15/19] fix --- .../subsystems/sideshooter/SideShooter.java | 229 ++---------------- .../sideshooter/SideShooterConstants.java | 14 +- 2 files changed, 24 insertions(+), 219 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 61187452a..d236f1bd0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -23,18 +23,28 @@ public static SideShooter getInstance() { return INSTANCE; } - private Rotation2d getAngleMotorPoison() { + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + + public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> generateAngleMotorProfile(targetAngle), + this::setTargetAngleMotorProfileTime, + (interrupted) -> {}, + () -> false, + this + ); + } + + private Rotation2d getAngleMotorposition() { double positionRevolution = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolution); } - private Double getAngleMotorVelocity() { + private Double getAngleMotorVelocityRotationsPerSecond() { return SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.getValue(); } - private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGeneration; - private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; } @@ -42,7 +52,7 @@ private double getAngleMotorProfileTime() { private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorPoison().getDegrees(), getAngleMotorVelocity())); + new TrapezoidProfile.State(getAngleMotorposition().getDegrees(), getAngleMotorVelocityRotationsPerSecond())); lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } @@ -55,7 +65,7 @@ private void setTargetAngleMotorProfileTime() { TrapezoidProfile.State targetGetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorPoison().getDegrees(), + getAngleMotorposition().getDegrees(), targetGetState.position ); double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( @@ -67,209 +77,4 @@ private void setTargetAngleMotorProfileTime() { angleMotor.setVoltage(pidOutPut = Feedforward); } - public Command getSetTargetShooterAngleCommand(Rotation2d angle) { - return new FunctionalCommand( - () -> generateAngleMotorProfile(angle), - this::setTargetAngleMotorProfileTime, - (interrupted) -> {}, - () -> false, - this - ); - } } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 2aebbb6b2..bf7327cf4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -22,7 +22,7 @@ public class SideShooterConstants { private static final int ENCODER_ID = 0; private static final double ANGLE_ENCODER_OFFSET = 0; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; - private static final AbsoluteSensorRangeValue ANGEL_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; @@ -33,10 +33,10 @@ public class SideShooterConstants { private static final CANcoder ANGLE_ENCODER = new CANcoder(ENCODER_ID); private static final double - MAX_ANGEL_VELOCITY = 600, - MAX_ANGEL_ACCELERATION = 500; + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500; static final TrapezoidProfile.Constraints ANGLE_Constraints = new TrapezoidProfile.Constraints( - MAX_ANGEL_VELOCITY, MAX_ANGEL_ACCELERATION + MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION ); private static final double @@ -85,7 +85,7 @@ private static void configureAngleMotor() { private static void configureAngleEncoder() { CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); - configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGEL_ENCODER_VALUE; + configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_VALUE; configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); @@ -102,8 +102,8 @@ public enum SideShooterState { final Rotation2d angle; final double shootingVoltage; - SideShooterState(Rotation2d angel, double shootingVoltage) { - this.angle = angel; + SideShooterState(Rotation2d angle, double shootingVoltage) { + this.angle = angle; this.shootingVoltage = shootingVoltage; } } From 1835e457eeb6bb71a1c7b5b38e9debf40381c397 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Tue, 5 Dec 2023 21:49:23 +0200 Subject: [PATCH 16/19] Update SideShooter.java --- .../trigon/robot/subsystems/sideshooter/SideShooter.java | 6 +++--- 1 file changed, 3 insertions(+), 3 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 d236f1bd0..3be471e14 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -36,7 +36,7 @@ public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { ); } - private Rotation2d getAngleMotorposition() { + private Rotation2d getAngleMotorPosition() { double positionRevolution = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolution); } @@ -52,7 +52,7 @@ private double getAngleMotorProfileTime() { private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorposition().getDegrees(), getAngleMotorVelocityRotationsPerSecond())); + new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocityRotationsPerSecond())); lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } @@ -65,7 +65,7 @@ private void setTargetAngleMotorProfileTime() { TrapezoidProfile.State targetGetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorposition().getDegrees(), + getAngleMotorPosition().getDegrees(), targetGetState.position ); double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( From 86e06bbc54b76bc7598f0b8251de00da7ac546a6 Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Fri, 22 Dec 2023 13:05:30 +0200 Subject: [PATCH 17/19] for ezra --- .../subsystems/sideshooter/SideShooter.java | 43 ++++++++++--------- .../sideshooter/SideShooterConstants.java | 11 ++--- 2 files changed, 27 insertions(+), 27 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 3be471e14..9591e9b00 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -11,38 +11,40 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.utilities.Conversions; import java.lang.annotation.Target; 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 TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; public static SideShooter getInstance() { return INSTANCE; } - - private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGeneration; + private final static SideShooter INSTANCE = new SideShooter(); public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( () -> generateAngleMotorProfile(targetAngle), this::setTargetAngleMotorProfileTime, - (interrupted) -> {}, + (interrupted) -> { + }, () -> false, this ); } private Rotation2d getAngleMotorPosition() { - double positionRevolution = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); - return Rotation2d.fromRotations(positionRevolution); + double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); } private Double getAngleMotorVelocityRotationsPerSecond() { - return SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.getValue(); + + return Conversions.revolutionsToDegrees(); } private double getAngleMotorProfileTime() { @@ -57,24 +59,25 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } + private void calculateAngleOutput{ + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleMotorPosition().getDegrees(), + targetState.position + ); + double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity), + targetState.position + ); + } + private void setTargetAngleMotorProfileTime() { if (angleMotorProfile == null) { angleMotor.stopMotor(); return; } - TrapezoidProfile.State targetGetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorPosition().getDegrees(), - targetGetState.position - ); - double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( - Units.degreesToRadians(targetGetState.position), - Units.degreesToRadians(targetGetState.velocity), - targetGetState.position - ); - - angleMotor.setVoltage(pidOutPut = Feedforward); } } 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 bf7327cf4..9889bccee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -17,17 +17,15 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public class SideShooterConstants { - private static final int SHOOTING_MOTOR_ID = 0; - private static final int ANGLE_MOTOR_ID = 0; - private static final int ENCODER_ID = 0; + private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 0, ENCODER_ID = 0; private static final double ANGLE_ENCODER_OFFSET = 0; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_MOTOR_INVERTED = false; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double VOLTAGE_COMPENSATION_SATURATION = 12; - private static final boolean ANGLE_MOTOR_INVERTED = false; static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); private static final CANcoder ANGLE_ENCODER = new CANcoder(ENCODER_ID); @@ -56,9 +54,7 @@ public class SideShooterConstants { ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); - static final StatusSignal - ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); - static final StatusSignal ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); static { configureAngleEncoder(); @@ -89,6 +85,7 @@ private static void configureAngleEncoder() { configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); From c39f5d418e6f9e16a5f21bfd081bdcdc28e75f0c Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Sat, 23 Dec 2023 21:52:18 +0200 Subject: [PATCH 18/19] give me the next think alredy --- .../subsystems/sideshooter/SideShooter.java | 33 +++++++------------ .../sideshooter/SideShooterCommands.java | 23 +++++++++++++ 2 files changed, 35 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.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 index 9591e9b00..0c7ef53a0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.Conversions; +import javax.swing.text.Position; import java.lang.annotation.Target; public class SideShooter extends SubsystemBase { @@ -26,32 +27,21 @@ public static SideShooter getInstance() { } private final static SideShooter INSTANCE = new SideShooter(); - public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { - return new FunctionalCommand( - () -> generateAngleMotorProfile(targetAngle), - this::setTargetAngleMotorProfileTime, - (interrupted) -> { - }, - () -> false, - this - ); - } - - private Rotation2d getAngleMotorPosition() { + Rotation2d getAngleMotorPosition() { double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolutions); } - private Double getAngleMotorVelocityRotationsPerSecond() { - - return Conversions.revolutionsToDegrees(); + Double getAngleMotorVelocityRotationsPerSecond() { + double perHundredMsToPerSecond = Conversions.perHundredMsToPerSecond(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + return Conversions.revolutionsToDegrees(perHundredMsToPerSecond); } - private double getAngleMotorProfileTime() { + double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; } - private void generateAngleMotorProfile(Rotation2d targetAngle) { + void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocityRotationsPerSecond())); @@ -59,7 +49,7 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); } - private void calculateAngleOutput{ + double calculateAngleOutput(){ TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( getAngleMotorPosition().getDegrees(), @@ -70,14 +60,15 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { Units.degreesToRadians(targetState.velocity), targetState.position ); + angleMotor.setVoltage(pidOutPut = Feedforward); + return Feedforward+pidOutPut; } - private void setTargetAngleMotorProfileTime() { + void setTargetAngleMotorProfileTime() { if (angleMotorProfile == null) { angleMotor.stopMotor(); return; } - - + angleMotor.setVoltage(calculateAngleOutput()); } } 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..d4fbe036d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,23 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; + +public class SideShooterCommands { + + public static SideShooterCommands getInstance() { + return getInstance(); + } + + + public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + () -> getSetTargetShooterAngleCommand(targetAngle), + (interrupted) -> {}, + () -> false, + SideShooter.getInstance() + ); + } +} From 65b62430cec80fe8630835763a5ee6cfd99e59cb Mon Sep 17 00:00:00 2001 From: azdaniel123 <99433174+azdaniel123@users.noreply.github.com> Date: Thu, 28 Dec 2023 16:14:00 +0200 Subject: [PATCH 19/19] hope that the last --- .../subsystems/sideshooter/SideShooter.java | 53 ++++++++----------- 1 file changed, 23 insertions(+), 30 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 0c7ef53a0..17df1fdcc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -3,65 +3,57 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.controller.ArmFeedforward; 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.Command; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.Conversions; -import javax.swing.text.Position; -import java.lang.annotation.Target; - public class SideShooter extends SubsystemBase { private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGeneration; + private double lastAngleMotorProfileGenerationTime; + + private final static SideShooter INSTANCE = new SideShooter(); public static SideShooter getInstance() { return INSTANCE; } - private final static SideShooter INSTANCE = new SideShooter(); - Rotation2d getAngleMotorPosition() { + double getAnglePosition() { double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); - return Rotation2d.fromRotations(positionRevolutions); + return positionRevolutions; } - Double getAngleMotorVelocityRotationsPerSecond() { - double perHundredMsToPerSecond = Conversions.perHundredMsToPerSecond(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); - return Conversions.revolutionsToDegrees(perHundredMsToPerSecond); + Double getAngleVelocityDegreesPerSecond() { + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } double getAngleMotorProfileTime() { - return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocityRotationsPerSecond())); + new TrapezoidProfile.State(getAnglePosition(), getAngleVelocityDegreesPerSecond())); - lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } - double calculateAngleOutput(){ - TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorPosition().getDegrees(), - targetState.position - ); - double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( - Units.degreesToRadians(targetState.position), - Units.degreesToRadians(targetState.velocity), - targetState.position - ); - angleMotor.setVoltage(pidOutPut = Feedforward); - return Feedforward+pidOutPut; + double calculateAngleOutput(TrapezoidProfile.State targetState) { + double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition(), + targetState.position + ); + double feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + targetState.velocity + ); + angleMotor.setVoltage(pidOutPut = feedforward); + return feedforward + pidOutPut; } void setTargetAngleMotorProfileTime() { @@ -69,6 +61,7 @@ void setTargetAngleMotorProfileTime() { angleMotor.stopMotor(); return; } - angleMotor.setVoltage(calculateAngleOutput()); + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + angleMotor.setVoltage(calculateAngleOutput(targetState)); } }