From de7cf24a51d487795ac3092d247a877a03fb8c4d Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 9 Nov 2023 12:40:44 +0200 Subject: [PATCH 01/21] did side shooter constanst --- .../sideshooter/SideShooterConstants.java | 77 +++++++++++++++++++ 1 file changed, 77 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..cc97a153e --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -0,0 +1,77 @@ +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.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; + +public class SideShooterConstants { + private static final double VOLTAGE_COMPENSATION = 12; + private static final int + SHOOTING_MOTOR_ID = 0, + ANGLE_MOTOR_ID = 1, + ANGLE_ENCODER_ID = 2; + + private static final double ANGLE_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + + private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_MOTOR_INVERTED = true; + private static final SensorDirectionValue ANGLE_ENCODER_INVERTED = SensorDirectionValue.Clockwise_Positive; + + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + + 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 CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + + static { + configureAngleEncoder(); + configureShootingMotor(); + configureAngleMotor(); + } + + private static void configureShootingMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + config.MotorOutput.Inverted = SHOOTING_MOTOR_INVERTED_VALUE; + config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; + SHOOTING_MOTOR.getConfigurator().apply(config); + } + + private static void configureAngleMotor() { + ANGLE_MOTOR.restoreFactoryDefaults(); + ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); + ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION); + } + private static void configureAngleEncoder(){ + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE; + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_INVERTED; + ANGLE_ENCODER.getConfigurator().apply(config); + } + + enum SideShooter{ + COLLECTOR(-15, -5), + MIDDLE(30, 5), + HIGH(60, 10); + + private double angle, power; + + SideShooter(double angle, double power) { + this.angle = angle; + this.power = power; + } + } +} + From dab5da71edf735394036d7c95c9fc6d6632aff4c Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 9 Nov 2023 13:00:36 +0200 Subject: [PATCH 02/21] added pid --- .../robot/subsystems/sideshooter/SideShooterConstants.java | 7 +++++++ 1 file changed, 7 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 cc97a153e..8c514cf5f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/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.PIDController; public class SideShooterConstants { private static final double VOLTAGE_COMPENSATION = 12; @@ -32,6 +33,12 @@ public class SideShooterConstants { static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + private static final double + P = 1, + I = 0, + D = 0; + static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + static { configureAngleEncoder(); configureShootingMotor(); From 7f4761330c67e555842afc4546d386b36c2f7534 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Mon, 13 Nov 2023 11:51:37 +0200 Subject: [PATCH 03/21] updated --- .../sideshooter/SideShooterConstants.java | 43 +++++++++---------- 1 file changed, 21 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 8c514cf5f..64b4d31b9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -13,26 +13,24 @@ import edu.wpi.first.math.controller.PIDController; public class SideShooterConstants { + private static final double VOLTAGE_COMPENSATION = 12; private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 1, ANGLE_ENCODER_ID = 2; - private static final double ANGLE_ENCODER_OFFSET = 0; - private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - - private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - private static final boolean ANGLE_MOTOR_INVERTED = true; - private static final SensorDirectionValue ANGLE_ENCODER_INVERTED = SensorDirectionValue.Clockwise_Positive; - - private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; - private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; - - 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 CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + private static final double ANGLE_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final double P = 1, I = 0, @@ -60,25 +58,26 @@ private static void configureAngleMotor() { ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION); } - private static void configureAngleEncoder(){ + + private static void configureAngleEncoder() { CANcoderConfiguration config = new CANcoderConfiguration(); config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE; config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; - config.MagnetSensor.SensorDirection = ANGLE_ENCODER_INVERTED; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(config); } - enum SideShooter{ - COLLECTOR(-15, -5), - MIDDLE(30, 5), - HIGH(60, 10); + public enum SideShooter { + COLLECTOR(-15, -5), + MIDDLE(30, 5), + HIGH(60, 10); - private double angle, power; + final double angle, power; - SideShooter(double angle, double power) { - this.angle = angle; - this.power = power; - } + SideShooter(double angle, double power) { + this.angle = angle; + this.power = power; + } } } From d7effcb84a358d33610ed4098c742e146897b308 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Mon, 13 Nov 2023 13:56:17 +0200 Subject: [PATCH 04/21] Update SideShooterConstants.java --- .../sideshooter/SideShooterConstants.java | 37 +++++++++++++------ 1 file changed, 25 insertions(+), 12 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 64b4d31b9..04a0d1b23 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,9 @@ 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 { @@ -20,22 +23,31 @@ public class SideShooterConstants { ANGLE_MOTOR_ID = 1, ANGLE_ENCODER_ID = 2; - static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); - static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); - static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); private static final double ANGLE_ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + + private static final double + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500; + + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); + private static final double - P = 1, - I = 0, - D = 0; - static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + ANGLE_MOTOR_P = 1, + 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); static { configureAngleEncoder(); @@ -68,13 +80,14 @@ private static void configureAngleEncoder() { } public enum SideShooter { - COLLECTOR(-15, -5), - MIDDLE(30, 5), - HIGH(60, 10); + COLLECTION(new Rotation2d(-15), -5), + MIDDLE(new Rotation2d(30), 5), + HIGH(new Rotation2d(60), 10); - final double angle, power; + final Rotation2d angle; + final double power; - SideShooter(double angle, double power) { + SideShooter(Rotation2d angle, double power) { this.angle = angle; this.power = power; } From bb32e20c4c09c02523f565e202f0f22ed31af603 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Tue, 14 Nov 2023 11:09:01 +0200 Subject: [PATCH 05/21] Update SideShooterConstants.java --- .../subsystems/sideshooter/SideShooterConstants.java | 12 +++--------- 1 file changed, 3 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 04a0d1b23..0953c1633 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -13,17 +13,12 @@ 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 double VOLTAGE_COMPENSATION = 12; private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 1, ANGLE_ENCODER_ID = 2; - - private static final double ANGLE_ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; @@ -40,7 +35,6 @@ public class SideShooterConstants { private static final double MAX_ANGLE_VELOCITY = 600, MAX_ANGLE_ACCELERATION = 500; - static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); private static final double @@ -80,9 +74,9 @@ private static void configureAngleEncoder() { } public enum SideShooter { - COLLECTION(new Rotation2d(-15), -5), - MIDDLE(new Rotation2d(30), 5), - HIGH(new Rotation2d(60), 10); + COLLECTION(Rotation2d.fromDegrees(-15), -5), + MIDDLE(Rotation2d.fromDegrees(30), 5), + HIGH(Rotation2d.fromDegrees(60), 10); final Rotation2d angle; final double power; From e4f2cd57db027108549f8d6c889355d29385c7b5 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Tue, 14 Nov 2023 19:14:21 +0200 Subject: [PATCH 06/21] Update SideShooterConstants.java --- .../subsystems/sideshooter/SideShooterConstants.java | 9 +++++---- 1 file changed, 5 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 0953c1633..67cc3c16e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -13,8 +13,9 @@ 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 double VOLTAGE_COMPENSATION = 12; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 1, @@ -62,7 +63,7 @@ private static void configureAngleMotor() { ANGLE_MOTOR.restoreFactoryDefaults(); ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); - ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); } private static void configureAngleEncoder() { @@ -73,7 +74,7 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.getConfigurator().apply(config); } - public enum SideShooter { + public enum SideShooterStates { COLLECTION(Rotation2d.fromDegrees(-15), -5), MIDDLE(Rotation2d.fromDegrees(30), 5), HIGH(Rotation2d.fromDegrees(60), 10); @@ -81,7 +82,7 @@ public enum SideShooter { final Rotation2d angle; final double power; - SideShooter(Rotation2d angle, double power) { + SideShooterStates(Rotation2d angle, double power) { this.angle = angle; this.power = power; } From 6389cc6d36a13ad6dcd32c0e1dab8baee18995bf Mon Sep 17 00:00:00 2001 From: libertyhar Date: Wed, 15 Nov 2023 09:04:03 +0200 Subject: [PATCH 07/21] Update SideShooterConstants.java --- .../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 67cc3c16e..3f32db2e2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -74,17 +74,17 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.getConfigurator().apply(config); } - public enum SideShooterStates { + public enum SideShooterState { COLLECTION(Rotation2d.fromDegrees(-15), -5), MIDDLE(Rotation2d.fromDegrees(30), 5), HIGH(Rotation2d.fromDegrees(60), 10); final Rotation2d angle; - final double power; + final double voltage; - SideShooterStates(Rotation2d angle, double power) { + SideShooterState(Rotation2d angle, double voltage) { this.angle = angle; - this.power = power; + this.voltage = voltage; } } } From 67b334a501333c4d38f4e04bf176e5f2c8b296d0 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Tue, 21 Nov 2023 16:12:59 +0200 Subject: [PATCH 08/21] finshed comand --- .../subsystems/sideshooter/SideShooter.java | 85 +++++++++++++++++++ .../sideshooter/SideShooterConstants.java | 19 +++++ 2 files changed, 104 insertions(+) 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..5eb16d5cd --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -0,0 +1,85 @@ +package frc.trigon.robot.subsystems.sideshooter; + + +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.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; + +public class SideShooter extends SubsystemBase { + private final static SideShooter INSTANCE = new SideShooter(); + + private final TalonFX SHOOTING_MOTOR = SideShooterConstants.SHOOTING_MOTOR; + private final CANSparkMax ANGLE_MOTOR = SideShooterConstants.ANGLE_MOTOR; + private final CANcoder ANGLE_ENCODER = SideShooterConstants.ANGLE_ENCODER; + + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime; + + public static SideShooter getInstance() { + return INSTANCE; + } + + private SideShooter() { + } + + private void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) + ); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + private Rotation2d getAnglePosition() { + return Rotation2d.fromRotations(SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue()); + } + private double getAngleVelocityDegreesPerSecond(){ + return Conversions.perHundredMsToPerSecond(Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue())); + } + + private double getAngleMotorProfileTime(){ + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; + } + private void setTargetAngleFromProfile(){ + if (angleMotorProfile==null){ + stop_angle_motor(); + } + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + calculateAngleMotorOutput(); + } + public Command getSetTargetShooterAngleCommand(Rotation2d angle){ + return new FunctionalCommand( + ()->generateAngleMotorProfile(angle), + this::setTargetAngleFromProfile, + (interrupted)->{}, + ()->false, + this + ); + } + + private void calculateAngleMotorOutput(){ + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + 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) + ); + ANGLE_MOTOR.setVoltage(pidOutput + feedforward); + } + private void stop_angle_motor(){ + ANGLE_MOTOR.stopMotor(); + } +} + 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 3f32db2e2..974c47027 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; @@ -10,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; @@ -44,6 +46,18 @@ public class SideShooterConstants { 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.5990, + ANGLE_MOTOR_KV = 0.5990, + ANGLE_MOTOR_KA = 0.5990, + ANGLE_MOTOR_KG = 0.5990; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + 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 { configureAngleEncoder(); configureShootingMotor(); @@ -57,6 +71,8 @@ private static void configureShootingMotor() { config.MotorOutput.Inverted = SHOOTING_MOTOR_INVERTED_VALUE; config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; SHOOTING_MOTOR.getConfigurator().apply(config); + + SHOOTING_MOTOR.optimizeBusUtilization(); } private static void configureAngleMotor() { @@ -72,6 +88,9 @@ private static void configureAngleEncoder() { config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(config); + + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); } public enum SideShooterState { From d53ff26b4dc91ffd425f3e7bbe2e9801012f0c78 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Wed, 22 Nov 2023 12:36:33 +0200 Subject: [PATCH 09/21] Update SideShooterConstants.java --- .../robot/subsystems/sideshooter/SideShooterConstants.java | 2 -- 1 file changed, 2 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 974c47027..d3b9f1de5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -26,11 +26,9 @@ public class SideShooterConstants { private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; - private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; - static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); From f3a9225274ad45d8b276d954bd26377ced435ab9 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Wed, 22 Nov 2023 15:51:42 +0200 Subject: [PATCH 10/21] fixed logika name --- .../subsystems/sideshooter/SideShooter.java | 44 +++++++++++-------- .../sideshooter/SideShooterConstants.java | 2 + 2 files changed, 27 insertions(+), 19 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 5eb16d5cd..e6a29983f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -23,11 +23,22 @@ public class SideShooter extends SubsystemBase { private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGenerationTime; + private SideShooter() { + } + public static SideShooter getInstance() { return INSTANCE; } - private SideShooter() { + public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> generateAngleMotorProfile(targetAngle), + this::setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); } private void generateAngleMotorProfile(Rotation2d targetAngle) { @@ -42,31 +53,25 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { private Rotation2d getAnglePosition() { return Rotation2d.fromRotations(SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue()); } - private double getAngleVelocityDegreesPerSecond(){ + + private double getAngleVelocityDegreesPerSecond() { return Conversions.perHundredMsToPerSecond(Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue())); } - private double getAngleMotorProfileTime(){ + private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private void setTargetAngleFromProfile(){ - if (angleMotorProfile==null){ - stop_angle_motor(); + + private void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + stopAngleMotor(); } + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - calculateAngleMotorOutput(); - } - public Command getSetTargetShooterAngleCommand(Rotation2d angle){ - return new FunctionalCommand( - ()->generateAngleMotorProfile(angle), - this::setTargetAngleFromProfile, - (interrupted)->{}, - ()->false, - this - ); + ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); } - private void calculateAngleMotorOutput(){ + private double calculateAngleMotorOutput() { TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( getAnglePosition().getDegrees(), @@ -76,9 +81,10 @@ private void calculateAngleMotorOutput(){ Units.degreesToRadians(targetState.position), Units.degreesToRadians(targetState.velocity) ); - ANGLE_MOTOR.setVoltage(pidOutput + feedforward); + return pidOutput + feedforward; } - private void stop_angle_motor(){ + + private void stopAngleMotor() { ANGLE_MOTOR.stopMotor(); } } 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 d3b9f1de5..974c47027 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -26,9 +26,11 @@ public class SideShooterConstants { private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); From 43b1bba95452eeef95071fa5410c44d6e6944a70 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 23 Nov 2023 13:02:21 +0200 Subject: [PATCH 11/21] fixed units --- .../frc/trigon/robot/subsystems/sideshooter/SideShooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e6a29983f..e80b94278 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -55,7 +55,7 @@ private Rotation2d getAnglePosition() { } private double getAngleVelocityDegreesPerSecond() { - return Conversions.perHundredMsToPerSecond(Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue())); + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } private double getAngleMotorProfileTime() { From a08c2755a39cad98d63cbb9164aabf11c442241c Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 23 Nov 2023 18:18:02 +0200 Subject: [PATCH 12/21] Update SideShooter.java --- .../subsystems/sideshooter/SideShooter.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 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 e80b94278..3cbdfd55d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -16,9 +16,9 @@ public class SideShooter extends SubsystemBase { private final static SideShooter INSTANCE = new SideShooter(); - private final TalonFX SHOOTING_MOTOR = SideShooterConstants.SHOOTING_MOTOR; - private final CANSparkMax ANGLE_MOTOR = SideShooterConstants.ANGLE_MOTOR; - private final CANcoder ANGLE_ENCODER = SideShooterConstants.ANGLE_ENCODER; + private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; + private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; + private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGenerationTime; @@ -50,6 +50,15 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } + private void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + stopAngleMotor(); + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + angleMotor.setVoltage(calculateAngleMotorOutput()); + } + private Rotation2d getAnglePosition() { return Rotation2d.fromRotations(SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue()); } @@ -62,15 +71,6 @@ private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private void setTargetAngleFromProfile() { - if (angleMotorProfile == null) { - stopAngleMotor(); - } - - TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); - } - private double calculateAngleMotorOutput() { TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( @@ -85,7 +85,7 @@ private double calculateAngleMotorOutput() { } private void stopAngleMotor() { - ANGLE_MOTOR.stopMotor(); + angleMotor.stopMotor(); } } From 14edb27c6c6487f9d7b03a064ed5347701352e10 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Sun, 26 Nov 2023 15:53:34 +0200 Subject: [PATCH 13/21] added command --- .../subsystems/sideshooter/SideShooter.java | 40 +++++++++++++------ .../sideshooter/SideShooterConstants.java | 8 +++- 2 files changed, 34 insertions(+), 14 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 3cbdfd55d..87e86fa82 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -1,6 +1,6 @@ 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; @@ -10,6 +10,7 @@ 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.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.Conversions; @@ -20,6 +21,8 @@ public class SideShooter extends SubsystemBase { private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; + private final VoltageOut shootingVoltageRequest = new VoltageOut(0,SideShooterConstants.FOC_ENABLE,false); + private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGenerationTime; @@ -41,6 +44,14 @@ public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { ); } + public Command getSetTargetShootingVoltageCommand(double voltage){ + return new StartEndCommand( + () -> setTargetShootingVoltage(voltage), + this::stopAngleMotor, + this + ); + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, @@ -53,12 +64,26 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { private void setTargetAngleFromProfile() { if (angleMotorProfile == null) { stopAngleMotor(); + return; } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); angleMotor.setVoltage(calculateAngleMotorOutput()); } + private double calculateAngleMotorOutput() { + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + 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() { return Rotation2d.fromRotations(SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue()); } @@ -71,17 +96,8 @@ private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private double calculateAngleMotorOutput() { - TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - 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 void setTargetShootingVoltage(double voltage){ + shootingMotor.setControl(shootingVoltageRequest.withOutput(voltage)); } private void stopAngleMotor() { 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 974c47027..dc3e2078a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -55,8 +55,11 @@ 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 final boolean FOC_ENABLE = true; static { configureAngleEncoder(); @@ -89,6 +92,7 @@ private static void configureAngleEncoder() { config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(config); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); ANGLE_ENCODER.optimizeBusUtilization(); } From fecff427ed0243ed75f7baf5867d3966f60d7afa Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 30 Nov 2023 10:18:27 +0200 Subject: [PATCH 14/21] added command --- .../subsystems/sideshooter/SideShooter.java | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 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 87e86fa82..4923efeb7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -8,10 +8,7 @@ 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.StartEndCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.utilities.Conversions; public class SideShooter extends SubsystemBase { @@ -21,7 +18,7 @@ public class SideShooter extends SubsystemBase { private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; - private final VoltageOut shootingVoltageRequest = new VoltageOut(0,SideShooterConstants.FOC_ENABLE,false); + private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLE, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGenerationTime; @@ -44,7 +41,7 @@ public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { ); } - public Command getSetTargetShootingVoltageCommand(double voltage){ + public Command getSetTargetShootingVoltageCommand(double voltage) { return new StartEndCommand( () -> setTargetShootingVoltage(voltage), this::stopAngleMotor, @@ -52,6 +49,24 @@ public Command getSetTargetShootingVoltageCommand(double voltage){ ); } + + + public Command getSetNotByOrderCommand(double voltage, Rotation2d targetAngle) { + return new ParallelCommandGroup( + getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShootingVoltageCommand(voltage) + ); + } + + private void iDoNotHaveName(double voltage, Rotation2d targetAngle, boolean byOrder) { + if (!byOrder) { + getSetNotByOrderCommand(voltage, targetAngle); + } else { + getSetTargetShooterAngleCommand(targetAngle); + getSetTargetShootingVoltageCommand(voltage); + } + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, @@ -96,7 +111,7 @@ private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private void setTargetShootingVoltage(double voltage){ + private void setTargetShootingVoltage(double voltage) { shootingMotor.setControl(shootingVoltageRequest.withOutput(voltage)); } From 711a5b8ec0e607d849e5c78f870f0832627b6ae7 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Fri, 1 Dec 2023 14:14:46 +0200 Subject: [PATCH 15/21] fixed command --- .../subsystems/sideshooter/SideShooter.java | 41 +++++++++---------- 1 file changed, 19 insertions(+), 22 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 4923efeb7..14f16e8d9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -30,6 +30,21 @@ public static SideShooter getInstance() { return INSTANCE; } + private Command getSetTargetStateCommand(boolean byOrder, Rotation2d targetAngle, double targetVoltage){ + if (!byOrder){ + return new ParallelCommandGroup( + getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + else{ + return new SequentialCommandGroup( + getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + } + public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( () -> generateAngleMotorProfile(targetAngle), @@ -41,32 +56,14 @@ public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { ); } - public Command getSetTargetShootingVoltageCommand(double voltage) { + public Command getSetTargetShootingVoltageCommand(double targetVoltage) { return new StartEndCommand( - () -> setTargetShootingVoltage(voltage), + () -> setTargetShootingVoltage(targetVoltage), this::stopAngleMotor, this ); } - - - public Command getSetNotByOrderCommand(double voltage, Rotation2d targetAngle) { - return new ParallelCommandGroup( - getSetTargetShooterAngleCommand(targetAngle), - getSetTargetShootingVoltageCommand(voltage) - ); - } - - private void iDoNotHaveName(double voltage, Rotation2d targetAngle, boolean byOrder) { - if (!byOrder) { - getSetNotByOrderCommand(voltage, targetAngle); - } else { - getSetTargetShooterAngleCommand(targetAngle); - getSetTargetShootingVoltageCommand(voltage); - } - } - private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, @@ -111,8 +108,8 @@ private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private void setTargetShootingVoltage(double voltage) { - shootingMotor.setControl(shootingVoltageRequest.withOutput(voltage)); + private void setTargetShootingVoltage(double targetVoltage) { + shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); } private void stopAngleMotor() { From 20dc4648b88c22388188bd436060d77be9754f91 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Mon, 4 Dec 2023 11:45:54 +0200 Subject: [PATCH 16/21] added command class --- .../subsystems/sideshooter/SideShooter.java | 48 +++---------------- .../sideshooter/SideShooterCommands.java | 38 +++++++++++++++ .../sideshooter/SideShooterConstants.java | 31 +++++------- 3 files changed, 57 insertions(+), 60 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 14f16e8d9..53fa6317a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -8,7 +8,7 @@ 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.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.Conversions; public class SideShooter extends SubsystemBase { @@ -18,53 +18,19 @@ public class SideShooter extends SubsystemBase { private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; - private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLE, false); + private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); private TrapezoidProfile angleMotorProfile = null; private double lastAngleMotorProfileGenerationTime; - private SideShooter() { - } - public static SideShooter getInstance() { return INSTANCE; } - private Command getSetTargetStateCommand(boolean byOrder, Rotation2d targetAngle, double targetVoltage){ - if (!byOrder){ - return new ParallelCommandGroup( - getSetTargetShooterAngleCommand(targetAngle), - getSetTargetShootingVoltageCommand(targetVoltage) - ); - } - else{ - return new SequentialCommandGroup( - getSetTargetShooterAngleCommand(targetAngle), - getSetTargetShootingVoltageCommand(targetVoltage) - ); - } - } - - public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { - return new FunctionalCommand( - () -> generateAngleMotorProfile(targetAngle), - this::setTargetAngleFromProfile, - (interrupted) -> { - }, - () -> false, - this - ); - } - - public Command getSetTargetShootingVoltageCommand(double targetVoltage) { - return new StartEndCommand( - () -> setTargetShootingVoltage(targetVoltage), - this::stopAngleMotor, - this - ); + private SideShooter() { } - private void generateAngleMotorProfile(Rotation2d targetAngle) { + void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), @@ -73,7 +39,7 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } - private void setTargetAngleFromProfile() { + void setTargetAngleFromProfile() { if (angleMotorProfile == null) { stopAngleMotor(); return; @@ -108,11 +74,11 @@ private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private void setTargetShootingVoltage(double targetVoltage) { + void setTargetShootingVoltage(double targetVoltage) { shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); } - private void stopAngleMotor() { + void stopAngleMotor() { angleMotor.stopMotor(); } } 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..94b2beacd --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,38 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.*; + +public class SideShooterCommands { + public Command getSetTargetStateCommand(Rotation2d targetAngle, double targetVoltage, boolean byOrder) { + if (!byOrder) { + return new ParallelCommandGroup( + getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + return new SequentialCommandGroup( + getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + + public static Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + () -> SideShooter.getInstance().setTargetAngleFromProfile(), + (interrupted) -> { + }, + () -> false, + SideShooter.getInstance() + ); + } + + public Command getSetTargetShootingVoltageCommand(double targetVoltage) { + return new StartEndCommand( + () -> SideShooter.getInstance().setTargetShootingVoltage(targetVoltage), + () -> SideShooter.getInstance().stopAngleMotor(), + SideShooter.getInstance() + ); + } +} 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 dc3e2078a..9106dc140 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -17,50 +17,43 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public class SideShooterConstants { + static final boolean FOC_ENABLED = true; private static final double VOLTAGE_COMPENSATION_SATURATION = 12; private static final int SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 1, ANGLE_ENCODER_ID = 2; + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); private static final double ANGLE_ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; - private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; - static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); - static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); - + static final StatusSignal + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); private static final double - MAX_ANGLE_VELOCITY = 600, - MAX_ANGLE_ACCELERATION = 500; + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); - private static final double ANGLE_MOTOR_P = 1, 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.5990, - ANGLE_MOTOR_KV = 0.5990, - ANGLE_MOTOR_KA = 0.5990, - ANGLE_MOTOR_KG = 0.5990; + ANGLE_MOTOR_KS = 0.5990, + ANGLE_MOTOR_KV = 0.5990, + ANGLE_MOTOR_KA = 0.5990, + ANGLE_MOTOR_KG = 0.5990; static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( - ANGLE_MOTOR_KS,ANGLE_MOTOR_KG,ANGLE_MOTOR_KV,ANGLE_MOTOR_KA + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); - static final StatusSignal - ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), - ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); - - static final boolean FOC_ENABLE = true; - static { configureAngleEncoder(); configureShootingMotor(); From 04deeb6b51254bc90bab4a0403ea44fe32c478cd Mon Sep 17 00:00:00 2001 From: libertyhar Date: Tue, 5 Dec 2023 16:45:37 +0200 Subject: [PATCH 17/21] fixed riviu --- .../subsystems/sideshooter/SideShooter.java | 24 ++++++++++--------- .../sideshooter/SideShooterCommands.java | 23 ++++++++++-------- .../sideshooter/SideShooterConstants.java | 7 ++++++ 3 files changed, 33 insertions(+), 21 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 53fa6317a..81c02a278 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -7,13 +7,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.Conversions; public class SideShooter extends SubsystemBase { private final static SideShooter INSTANCE = new SideShooter(); - private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; @@ -30,13 +28,17 @@ public static SideShooter getInstance() { private SideShooter() { } + boolean ifGotToAngle(double targetAngle){ + return targetAngle == getAnglePosition().getDegrees(); + } + void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( SideShooterConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) ); - lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } void setTargetAngleFromProfile() { @@ -49,6 +51,14 @@ void setTargetAngleFromProfile() { angleMotor.setVoltage(calculateAngleMotorOutput()); } + void setTargetShootingVoltage(double targetVoltage) { + shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); + } + + void stopAngleMotor() { + angleMotor.stopMotor(); + } + private double calculateAngleMotorOutput() { TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( @@ -73,13 +83,5 @@ private double getAngleVelocityDegreesPerSecond() { private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - - void setTargetShootingVoltage(double targetVoltage) { - shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); - } - - void stopAngleMotor() { - angleMotor.stopMotor(); - } } 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 94b2beacd..5ffe252ca 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -2,37 +2,40 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.utilities.Commands; public class SideShooterCommands { - public Command getSetTargetStateCommand(Rotation2d targetAngle, double targetVoltage, boolean byOrder) { + + private static final SideShooter sideShooter = SideShooter.getInstance(); + public static Command getSetTargetStateCommand(Rotation2d targetAngle, double targetVoltage, boolean byOrder) { if (!byOrder) { return new ParallelCommandGroup( - getSetTargetShooterAngleCommand(targetAngle), + Commands.removeRequirements(getSetTargetShooterAngleCommand(targetAngle)), getSetTargetShootingVoltageCommand(targetVoltage) ); } return new SequentialCommandGroup( - getSetTargetShooterAngleCommand(targetAngle), + getSetTargetShooterAngleCommand(targetAngle).until(()->sideShooter.ifGotToAngle(targetAngle.getDegrees())), getSetTargetShootingVoltageCommand(targetVoltage) ); } public static Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( - () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), - () -> SideShooter.getInstance().setTargetAngleFromProfile(), + () -> sideShooter.generateAngleMotorProfile(targetAngle), + () -> sideShooter.setTargetAngleFromProfile(), (interrupted) -> { }, () -> false, - SideShooter.getInstance() + sideShooter ); } - public Command getSetTargetShootingVoltageCommand(double targetVoltage) { + public static Command getSetTargetShootingVoltageCommand(double targetVoltage) { return new StartEndCommand( - () -> SideShooter.getInstance().setTargetShootingVoltage(targetVoltage), - () -> SideShooter.getInstance().stopAngleMotor(), - SideShooter.getInstance() + () -> sideShooter.setTargetShootingVoltage(targetVoltage), + () -> sideShooter.stopAngleMotor(), + sideShooter ); } } 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 9106dc140..29703fc88 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -28,23 +28,30 @@ public class SideShooterConstants { private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + private static final double MAX_ANGLE_VELOCITY = 600, MAX_ANGLE_ACCELERATION = 500; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); + private static final double ANGLE_MOTOR_P = 1, 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.5990, ANGLE_MOTOR_KV = 0.5990, From b30d25c83d86d1c3f79806f7e923aa0a6966a86d Mon Sep 17 00:00:00 2001 From: libertyhar Date: Tue, 5 Dec 2023 18:25:32 +0200 Subject: [PATCH 18/21] fixxed --- .../subsystems/sideshooter/SideShooter.java | 7 +++++-- .../sideshooter/SideShooterCommands.java | 16 ++++++++-------- .../sideshooter/SideShooterConstants.java | 1 - 3 files changed, 13 insertions(+), 11 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 81c02a278..6bafe6e91 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -28,8 +28,11 @@ public static SideShooter getInstance() { private SideShooter() { } - boolean ifGotToAngle(double targetAngle){ - return targetAngle == getAnglePosition().getDegrees(); + boolean ifGotToAngle(double atAngle){ + if (Math.abs(atAngle - getAnglePosition().getDegrees()) <= 1){ + return true; + }; + return false; } void generateAngleMotorProfile(Rotation2d targetAngle) { 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 5ffe252ca..fc6030871 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -5,8 +5,8 @@ import frc.trigon.robot.utilities.Commands; public class SideShooterCommands { + private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); - private static final SideShooter sideShooter = SideShooter.getInstance(); public static Command getSetTargetStateCommand(Rotation2d targetAngle, double targetVoltage, boolean byOrder) { if (!byOrder) { return new ParallelCommandGroup( @@ -15,27 +15,27 @@ public static Command getSetTargetStateCommand(Rotation2d targetAngle, double ta ); } return new SequentialCommandGroup( - getSetTargetShooterAngleCommand(targetAngle).until(()->sideShooter.ifGotToAngle(targetAngle.getDegrees())), + getSetTargetShooterAngleCommand(targetAngle).until(()-> SIDE_SHOOTER.ifGotToAngle(targetAngle.getDegrees())), getSetTargetShootingVoltageCommand(targetVoltage) ); } public static Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( - () -> sideShooter.generateAngleMotorProfile(targetAngle), - () -> sideShooter.setTargetAngleFromProfile(), + () -> SIDE_SHOOTER.generateAngleMotorProfile(targetAngle), + () -> SIDE_SHOOTER.setTargetAngleFromProfile(), (interrupted) -> { }, () -> false, - sideShooter + SIDE_SHOOTER ); } public static Command getSetTargetShootingVoltageCommand(double targetVoltage) { return new StartEndCommand( - () -> sideShooter.setTargetShootingVoltage(targetVoltage), - () -> sideShooter.stopAngleMotor(), - sideShooter + () -> SIDE_SHOOTER.setTargetShootingVoltage(targetVoltage), + () -> SIDE_SHOOTER.stopAngleMotor(), + SIDE_SHOOTER ); } } 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 29703fc88..fb65401dd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -49,7 +49,6 @@ public class SideShooterConstants { ANGLE_MOTOR_P = 1, 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 From 3befdaf0af95b4a920568afa49f4d40b28619ca7 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Wed, 20 Dec 2023 23:03:25 +0200 Subject: [PATCH 19/21] added constanst and started the subsystem by adding angle profile --- .../frc/trigon/robot/subsystems/arm/Arm.java | 75 +++++++++ .../robot/subsystems/arm/ArmConstants.java | 145 ++++++++++++++++++ .../sideshooter/SideShooterConstants.java | 3 +- 3 files changed, 221 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/ArmConstants.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..2216fabcf --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,75 @@ +package frc.trigon.robot.subsystems.arm; + + +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.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.utilities.Conversions; + +public class Arm extends SubsystemBase { + + private final CANSparkMax MASTER_ANGLE_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR; + private final CANSparkMax FOLLOWER_ANGLE_MOTOR = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final static Arm INSTANCE = new Arm(); + + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime; + + public static Arm getInstance() { + return INSTANCE; + } + + private Arm(){} + + private void generateAngleMotorProfile(Rotation2d targetAngle){ + angleMotorProfile = new TrapezoidProfile( + ArmConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(),0), + new TrapezoidProfile.State(getAngleEncoderPosition().getDegrees(), getAngleVelocityDegreesPerSecond()) + ); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + private void setTargetAngleFromProfile(){ + if (angleMotorProfile == null){ + stopAngleMotors(); + return; + } + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + MASTER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); + FOLLOWER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); + } + + private double calculateAngleMotorOutput(){ + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleEncoderPosition().getDegrees(), + targetState.position + ); + double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return pidOutput + feedforward; + } + + private double getAngleMotorProfileTime(){ + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; + } + + private Rotation2d getAngleEncoderPosition(){ + return Rotation2d.fromRotations(ArmConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue()); + } + + private double getAngleVelocityDegreesPerSecond(){ + return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + } + + private void stopAngleMotors(){ + MASTER_ANGLE_MOTOR.stopMotor(); + FOLLOWER_ANGLE_MOTOR.stopMotor(); + } +} + 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..59cc1877c --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,145 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.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.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +public class ArmConstants { + + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + + private static final int + MASTER_ANGLE_MOTOR_ID = 0, + FOLLOWER_ANGLE_MOTOR_ID = 1, + MASTER_ELEVATOR_MOTOR_ID = 2, + FOLLOWER_ELEVATOR_MOTOR_ID = 3, + ANGLE_ENCODER_ID = 4, + ELEVATOR_TALON_SRX_ENCODER_ID = 5; + static final CANSparkMax + MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + private static final CANSparkMax.IdleMode + MASTER_ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + MASTER_ELEVATOR_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ELEVATOR_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final boolean + MASTER_ANGLE_MOTOR_INVERTED = false, + FOLLOWER_ANGLE_MOTOR_INVERTED = false, + MASTER_ELEVATOR_MOTOR_INVERTED = false, + FOLLOWER_ELEVATOR_MOTOR_INVERTED = true; + + private static final double ANGLE_ENCODER_OFFSET = 0; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonSRX ELEVATOR_TALON_SRX_ENCODER = new TalonSRX(ELEVATOR_TALON_SRX_ENCODER_ID); + private static final double MAG_ENCODER_OFF_SET = 0; + + private static final double + ANGLE_P = 1, + ANGLE_I = 0, + ANGLE_D = 0, + ELEVATOR_P = 1, + ELEVATOR_I = 0, + ELEVATOR_D = 0; + 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); + + private static final double + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY,MAX_ANGLE_ACCELERATION); + static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static final StatusSignal ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + private static final double + ANGLE_MOTOR_KS = 0.5990, + ANGLE_MOTOR_KV = 0.5990, + ANGLE_MOTOR_KA = 0.5990, + ANGLE_MOTOR_KG = 0.5990; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); + + private static void configureMasterAngleMotor() { + MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); + MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_MOTOR_IDLE_MODE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_MOTOR_INVERTED); + MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureFollowerAngleMotor() { + FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_MOTOR_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_MOTOR_INVERTED); + FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureMasterElevatorMotor() { + MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ELEVATOR_MOTOR_IDLE_MODE); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_MOTOR_INVERTED); + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureFollowerElevatorMotor() { + FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ELEVATOR_MOTOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_MOTOR_INVERTED); + FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleCanCoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE; + ANGLE_ENCODER.getConfigurator().apply(config); + + ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + private static void configureElevatorMagEncoder() { + ELEVATOR_TALON_SRX_ENCODER.configFactoryDefault(); + ELEVATOR_TALON_SRX_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + ELEVATOR_TALON_SRX_ENCODER.setSensorPhase(false); + ELEVATOR_TALON_SRX_ENCODER.setSelectedSensorPosition(MAG_ENCODER_OFF_SET, 0, 10); + } + + static { + configureFollowerAngleMotor(); + configureFollowerElevatorMotor(); + configureMasterAngleMotor(); + configureMasterElevatorMotor(); + configureAngleCanCoder(); + configureElevatorMagEncoder(); + } + + public enum ArmState { + COLLECTION(Rotation2d.fromDegrees(-15), 0), + MIDDLE(Rotation2d.fromDegrees(30), 30), + HIGH(Rotation2d.fromDegrees(60), 45); + + final Rotation2d angle; + final double elevatorPosition; + + ArmState(Rotation2d angle, double elevatorPosition) { + this.angle = angle; + this.elevatorPosition = elevatorPosition; + } + } +} 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 fb65401dd..c1ec80c79 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -23,18 +23,17 @@ public class SideShooterConstants { SHOOTING_MOTOR_ID = 0, ANGLE_MOTOR_ID = 1, ANGLE_ENCODER_ID = 2; - static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); private static final double ANGLE_ENCODER_OFFSET = 0; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final boolean ANGLE_MOTOR_INVERTED = false; - private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), From 5634e7b8143cebb53c71af7a3fe455d9f666462c Mon Sep 17 00:00:00 2001 From: libertyhar Date: Thu, 21 Dec 2023 20:04:46 +0200 Subject: [PATCH 20/21] added ArmCommands --- .../frc/trigon/robot/subsystems/arm/Arm.java | 68 +++++++++++++++++-- .../robot/subsystems/arm/ArmCommands.java | 31 +++++++++ .../robot/subsystems/arm/ArmConstants.java | 20 ++++-- .../subsystems/sideshooter/SideShooter.java | 4 +- 4 files changed, 111 insertions(+), 12 deletions(-) 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 index 2216fabcf..8866885d2 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,7 @@ 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; @@ -10,12 +11,18 @@ public class Arm extends SubsystemBase { - private final CANSparkMax MASTER_ANGLE_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR; - private final CANSparkMax FOLLOWER_ANGLE_MOTOR = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final CANSparkMax + MASTER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR, + FOLLOWER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR, + MASTER_ANGLE_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR, + FOLLOWER_ANGLE_MOTOR = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final TalonSRX ELEVATOR_TALON_SRX_ENCODER = ArmConstants.ELEVATOR_TALON_SRX_ENCODER; private final static Arm INSTANCE = new Arm(); - private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGenerationTime; + private TrapezoidProfile + elevatorMotorProfile = null, + angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime,lastElevatorMotorProfileGenerationTime; public static Arm getInstance() { return INSTANCE; @@ -23,7 +30,9 @@ public static Arm getInstance() { private Arm(){} - private void generateAngleMotorProfile(Rotation2d targetAngle){ + + + void generateAngleMotorProfile(Rotation2d targetAngle){ angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINTS, new TrapezoidProfile.State(targetAngle.getDegrees(),0), @@ -32,7 +41,7 @@ private void generateAngleMotorProfile(Rotation2d targetAngle){ lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } - private void setTargetAngleFromProfile(){ + void setTargetAngleFromProfile(){ if (angleMotorProfile == null){ stopAngleMotors(); return; @@ -55,6 +64,48 @@ private double calculateAngleMotorOutput(){ return pidOutput + feedforward; } + void generateElevatorMotorProfile(Rotation2d targetElevatorPosition){ + elevatorMotorProfile = new TrapezoidProfile( + ArmConstants.ELEVATOR_CONSTRAINTS, + new TrapezoidProfile.State(targetElevatorPosition.getDegrees(),0), + new TrapezoidProfile.State(getElevatorPositionSecondPerMeter(),getElevatorVelocityRevolutionPerSecond()) + ); + lastElevatorMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + void setTargetElevatorFromProfile(){ + if (elevatorMotorProfile == null) { + stopElevatorMotors(); + return; + } + MASTER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput()); + FOLLOWER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput()); + } + + private double calculateElevatorMotorOutput(){ + TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorPositionSecondPerMeter(), + targetState.position + ); + double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return pidOutput + feedforward; + } + + private double getElevatorMotorProfileTime(){ + return timer.getFPGATimestamp() - lastElevatorMotorProfileGenerationTime; + } + + private double getElevatorPositionSecondPerMeter(){ + return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ArmConstants.METER_PER_REVOLUTION*ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorPosition())); + } + private double getElevatorVelocityRevolutionPerSecond(){ + return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorVelocity())); + } + private double getAngleMotorProfileTime(){ return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } @@ -71,5 +122,10 @@ private void stopAngleMotors(){ MASTER_ANGLE_MOTOR.stopMotor(); FOLLOWER_ANGLE_MOTOR.stopMotor(); } + + private void stopElevatorMotors(){ + MASTER_ELEVATOR_MOTOR.stopMotor(); + FOLLOWER_ELEVATOR_MOTOR.stopMotor(); + } } 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..8b4664a21 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -0,0 +1,31 @@ +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; + +public class ArmCommands { + private static final Arm ARM = Arm.getInstance(); + + public static Command getSetTargetArmAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> ARM.generateAngleMotorProfile(targetAngle), + () -> ARM.setTargetAngleFromProfile(), + (interrupted) -> { + }, + () -> false, + ARM + ); + } + + public static Command getSetTargetArmElevatorCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> ARM.generateAngleMotorProfile(targetAngle), + () -> ARM.setTargetAngleFromProfile(), + (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 59cc1877c..f20e00148 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -47,6 +47,7 @@ public class ArmConstants { static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); static final TalonSRX ELEVATOR_TALON_SRX_ENCODER = new TalonSRX(ELEVATOR_TALON_SRX_ENCODER_ID); private static final double MAG_ENCODER_OFF_SET = 0; + static final double METER_PER_REVOLUTION = 1; private static final double ANGLE_P = 1, @@ -61,8 +62,12 @@ public class ArmConstants { private static final double MAX_ANGLE_VELOCITY = 600, - MAX_ANGLE_ACCELERATION = 500; - static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY,MAX_ANGLE_ACCELERATION); + MAX_ANGLE_ACCELERATION = 500, + MAX_ELEVATOR_VELOCITY = 600, + MAX_ELEVATOR_ACCELERATION = 500; + static final TrapezoidProfile.Constraints + ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY,MAX_ANGLE_ACCELERATION), + ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY,MAX_ELEVATOR_ACCELERATION); static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); static final StatusSignal ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); @@ -70,9 +75,14 @@ public class ArmConstants { ANGLE_MOTOR_KS = 0.5990, ANGLE_MOTOR_KV = 0.5990, ANGLE_MOTOR_KA = 0.5990, - ANGLE_MOTOR_KG = 0.5990; - static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ); - + ANGLE_MOTOR_KG = 0.5990, + ELEVATOR_MOTOR_KS = 0.5990, + ELEVATOR_MOTOR_KV = 0.5990, + ELEVATOR_MOTOR_KA = 0.5990, + ELEVATOR_MOTOR_KG = 0.5990; + static final ArmFeedforward + ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ), + ELEVATOR_MOTOR_FEEDFORWARD = new ArmFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA ); private static void configureMasterAngleMotor() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_MOTOR_IDLE_MODE); 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 6bafe6e91..36abaa853 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -18,7 +18,9 @@ public class SideShooter extends SubsystemBase { private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); - private TrapezoidProfile angleMotorProfile = null; + private TrapezoidProfile + angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime; public static SideShooter getInstance() { From 3ee8694f9fea4d6eee1a5cd9a9b845029c565803 Mon Sep 17 00:00:00 2001 From: libertyhar Date: Sun, 24 Dec 2023 11:05:13 +0200 Subject: [PATCH 21/21] added somting --- .../frc/trigon/robot/subsystems/arm/Arm.java | 63 ++++++++++--------- .../robot/subsystems/arm/ArmCommands.java | 4 +- .../robot/subsystems/arm/ArmConstants.java | 15 ++--- .../subsystems/sideshooter/SideShooter.java | 13 ++-- .../sideshooter/SideShooterCommands.java | 2 +- .../sideshooter/SideShooterConstants.java | 1 + 6 files changed, 50 insertions(+), 48 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 8866885d2..24fd03869 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,4 @@ package frc.trigon.robot.subsystems.arm; - - import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Rotation2d; @@ -10,7 +8,6 @@ import frc.trigon.robot.utilities.Conversions; public class Arm extends SubsystemBase { - private final CANSparkMax MASTER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR, FOLLOWER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR, @@ -22,40 +19,43 @@ public class Arm extends SubsystemBase { private TrapezoidProfile elevatorMotorProfile = null, angleMotorProfile = null; - private double lastAngleMotorProfileGenerationTime,lastElevatorMotorProfileGenerationTime; + private double lastAngleMotorProfileGenerationTime, lastElevatorMotorProfileGenerationTime; public static Arm getInstance() { return INSTANCE; } - private Arm(){} - - + private Arm() { + } - void generateAngleMotorProfile(Rotation2d targetAngle){ + void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINTS, - new TrapezoidProfile.State(targetAngle.getDegrees(),0), + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAngleEncoderPosition().getDegrees(), getAngleVelocityDegreesPerSecond()) ); - lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); } - void setTargetAngleFromProfile(){ - if (angleMotorProfile == null){ + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { stopAngleMotors(); return; } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + setTargetAngleVoltage(); + } + + private void setTargetAngleVoltage(){ MASTER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); FOLLOWER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput()); } - private double calculateAngleMotorOutput(){ + private double calculateAngleMotorOutput() { TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleEncoderPosition().getDegrees(), - targetState.position + getAngleEncoderPosition().getDegrees(), + targetState.position ); double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( Units.degreesToRadians(targetState.position), @@ -64,25 +64,29 @@ private double calculateAngleMotorOutput(){ return pidOutput + feedforward; } - void generateElevatorMotorProfile(Rotation2d targetElevatorPosition){ + void generateElevatorMotorProfile(Rotation2d targetElevatorPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINTS, - new TrapezoidProfile.State(targetElevatorPosition.getDegrees(),0), - new TrapezoidProfile.State(getElevatorPositionSecondPerMeter(),getElevatorVelocityRevolutionPerSecond()) + new TrapezoidProfile.State(targetElevatorPosition.getDegrees(), 0), + new TrapezoidProfile.State(getElevatorPositionSecondPerMeter(), getElevatorVelocityRevolutionPerSecond()) ); lastElevatorMotorProfileGenerationTime = Timer.getFPGATimestamp(); } - void setTargetElevatorFromProfile(){ + void setTargetElevatorFromProfile() { if (elevatorMotorProfile == null) { stopElevatorMotors(); return; } + setTargetElevatorVoltage(); + } + + private void setTargetElevatorVoltage(){ MASTER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput()); FOLLOWER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput()); } - private double calculateElevatorMotorOutput(){ + private double calculateElevatorMotorOutput() { TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( getElevatorPositionSecondPerMeter(), @@ -95,35 +99,36 @@ private double calculateElevatorMotorOutput(){ return pidOutput + feedforward; } - private double getElevatorMotorProfileTime(){ + private double getElevatorMotorProfileTime() { return timer.getFPGATimestamp() - lastElevatorMotorProfileGenerationTime; } - private double getElevatorPositionSecondPerMeter(){ - return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ArmConstants.METER_PER_REVOLUTION*ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorPosition())); + private double getElevatorPositionSecondPerMeter() { + return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ArmConstants.METER_PER_REVOLUTION * ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorPosition())); } - private double getElevatorVelocityRevolutionPerSecond(){ + + private double getElevatorVelocityRevolutionPerSecond() { return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorVelocity())); } - private double getAngleMotorProfileTime(){ + private double getAngleMotorProfileTime() { return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; } - private Rotation2d getAngleEncoderPosition(){ + private Rotation2d getAngleEncoderPosition() { return Rotation2d.fromRotations(ArmConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue()); } - private double getAngleVelocityDegreesPerSecond(){ + private double getAngleVelocityDegreesPerSecond() { return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); } - private void stopAngleMotors(){ + private void stopAngleMotors() { MASTER_ANGLE_MOTOR.stopMotor(); FOLLOWER_ANGLE_MOTOR.stopMotor(); } - private void stopElevatorMotors(){ + private void stopElevatorMotors() { MASTER_ELEVATOR_MOTOR.stopMotor(); FOLLOWER_ELEVATOR_MOTOR.stopMotor(); } 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 8b4664a21..6c16b8201 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -20,8 +20,8 @@ public static Command getSetTargetArmAngleCommand(Rotation2d targetAngle) { public static Command getSetTargetArmElevatorCommand(Rotation2d targetAngle) { return new FunctionalCommand( - () -> ARM.generateAngleMotorProfile(targetAngle), - () -> ARM.setTargetAngleFromProfile(), + () -> ARM.generateElevatorMotorProfile(targetAngle), + () -> ARM.setTargetElevatorFromProfile(), (interrupted) -> { }, () -> false, 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 f20e00148..41797511d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -66,8 +66,8 @@ public class ArmConstants { MAX_ELEVATOR_VELOCITY = 600, MAX_ELEVATOR_ACCELERATION = 500; static final TrapezoidProfile.Constraints - ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY,MAX_ANGLE_ACCELERATION), - ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY,MAX_ELEVATOR_ACCELERATION); + ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), + ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); static final StatusSignal ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); static final StatusSignal ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); @@ -81,8 +81,9 @@ public class ArmConstants { ELEVATOR_MOTOR_KA = 0.5990, ELEVATOR_MOTOR_KG = 0.5990; static final ArmFeedforward - ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA ), - ELEVATOR_MOTOR_FEEDFORWARD = new ArmFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA ); + ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA), + ELEVATOR_MOTOR_FEEDFORWARD = new ArmFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA); + private static void configureMasterAngleMotor() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_MOTOR_IDLE_MODE); @@ -145,11 +146,11 @@ public enum ArmState { HIGH(Rotation2d.fromDegrees(60), 45); final Rotation2d angle; - final double elevatorPosition; + final double elevatorPositionMeters; - ArmState(Rotation2d angle, double elevatorPosition) { + ArmState(Rotation2d angle, double elevatorPositionMeters) { this.angle = angle; - this.elevatorPosition = 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 36abaa853..a8b6d8100 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -16,11 +16,9 @@ public class SideShooter extends SubsystemBase { private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; private final CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; - private final VoltageOut shootingVoltageRequest = new VoltageOut(0, SideShooterConstants.FOC_ENABLED, false); - + private static final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SideShooterConstants.FOC_ENABLED); private TrapezoidProfile angleMotorProfile = null; - private double lastAngleMotorProfileGenerationTime; public static SideShooter getInstance() { @@ -30,11 +28,8 @@ public static SideShooter getInstance() { private SideShooter() { } - boolean ifGotToAngle(double atAngle){ - if (Math.abs(atAngle - getAnglePosition().getDegrees()) <= 1){ - return true; - }; - return false; + boolean atAngle(Rotation2d atAngle){ + return Math.abs(atAngle.getDegrees() - getAnglePosition().getDegrees()) <= 1; } void generateAngleMotorProfile(Rotation2d targetAngle) { @@ -57,7 +52,7 @@ void setTargetAngleFromProfile() { } void setTargetShootingVoltage(double targetVoltage) { - shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); + shootingMotor.setControl(driveVoltageRequest.withOutput(targetVoltage)); } void stopAngleMotor() { 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 fc6030871..efeeb31b8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -15,7 +15,7 @@ public static Command getSetTargetStateCommand(Rotation2d targetAngle, double ta ); } return new SequentialCommandGroup( - getSetTargetShooterAngleCommand(targetAngle).until(()-> SIDE_SHOOTER.ifGotToAngle(targetAngle.getDegrees())), + getSetTargetShooterAngleCommand(targetAngle).until(()-> SIDE_SHOOTER.atAngle(Rotation2d.fromRadians(targetAngle.getDegrees()))), getSetTargetShootingVoltageCommand(targetVoltage) ); } 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 c1ec80c79..842938eed 100644 --- a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;