From 5ad3e66e28277d7c795f476898ce905bf1ebb20e Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 9 Nov 2023 19:29:46 +0200 Subject: [PATCH 01/48] Constants --- .vscode/launch.json | 12 +- .../robot/subsystems/arm/ArmConstants.java | 107 ++++++++++++++++++ 2 files changed, 116 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java diff --git a/.vscode/launch.json b/.vscode/launch.json index e6ca564b8..76cb0e93c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc.trigon.robot.Main", + "projectName": "" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } \ No newline at end of file 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..e9cb0ed7c --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,107 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.DeviceIdentifier; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.PIDController; + +public class ArmConstants { + private static final int + MASTER_ANGLE_MOTOR_ID = 0, + FOLLOWER_ANGLE_MOTOR_ID = 1, + MASTER_ELEVATOR_MOTOR_ID = 2, + FOLLOWER_ELEVATOR_MOTOR_ID = 3, + MAG_ENCODER_ID = 4, + ANGLE_CANCODER_ID = 5; + + private static final double + MASTER_ANGLE_OFFSET_VALUE = 0.0, + FOLLOWER_ANGLE_OFFSET_VALUE = 0.0, + MASTER_ELEVATOR_OFFSET_VALUE = 0.0, + FOLLOWER_ELEVATOR_OFFSET_VALUE = 0.0, + MAG_ENCODER_OFFSET = 0.0, + ANGLE_CANCODER_OFFSET = 0.0; + + private static final boolean + MASTER_ANGLE_INVERTED_VALUE = true, + FOLLOWER_ANGLE_INVERTED_VALUE = true, + MASTER_ELEVATOR_INVERTED_VALUE = false, + FOLLOWER_ELEVATOR_INVERTED_VALUE = true, + MAG_ENCODER_INVERTED_VALUE = true; + private static final SensorDirectionValue CANCODER_INVERTED_VALUE = SensorDirectionValue.Clockwise_Positive; + + private static final double + P = 1.32, + I = 3.1, + D = 0; + PIDController PID_CONTROLLER = new PIDController(P,I,D); + + private static final CANSparkMax.IdleMode + MASTER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + MASTER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + + private 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 TalonSRX MAG_ENCODER = new TalonSRX(MAG_ENCODER_ID); + private static final CANcoder CANCODER = new CANcoder(ANGLE_CANCODER_ID); + + private static void config_elevator_motors(){ + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); + MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); + + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE); + + MAG_ENCODER.setSelectedSensorPosition(MAG_ENCODER.getSelectedSensorPosition() - MAG_ENCODER_OFFSET); + MAG_ENCODER.setSensorPhase(MAG_ENCODER_INVERTED_VALUE); + + } + + + private static void config_angle_motors(){ + MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); + MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); + + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE); + + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; + config.MagnetSensor.SensorDirection = CANCODER_INVERTED_VALUE; + CANCODER.getConfigurator().apply(config); + } + + static { + config_angle_motors(); + config_elevator_motors(); + } + public enum ArmStates { + FIRST_STATE(43, 1), + SECOND_STATE(50, 2), + THIRD_STATE(100, 3); + final double rotation2d; + final int elevatorPosition; + + ArmStates(double rotation2d, int elevatorPosition){ + this.elevatorPosition = elevatorPosition; + this.rotation2d = rotation2d; + } + + } + +} From 8e2b938ff293296fae26432fde5b40c065c69221 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 9 Nov 2023 19:32:47 +0200 Subject: [PATCH 02/48] Update ArmConstants.java --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index e9cb0ed7c..363cf958c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -71,7 +71,6 @@ private static void config_elevator_motors(){ } - private static void config_angle_motors(){ MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); From 99e624e613e1a2c29aa51e454e0d09e76ddd2660 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 9 Nov 2023 19:34:41 +0200 Subject: [PATCH 03/48] Update ArmConstants.java --- .../robot/subsystems/arm/ArmConstants.java | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 363cf958c..fd852d9b3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -15,12 +15,12 @@ public class ArmConstants { private static final int - MASTER_ANGLE_MOTOR_ID = 0, - FOLLOWER_ANGLE_MOTOR_ID = 1, - MASTER_ELEVATOR_MOTOR_ID = 2, - FOLLOWER_ELEVATOR_MOTOR_ID = 3, - MAG_ENCODER_ID = 4, - ANGLE_CANCODER_ID = 5; + MASTER_ANGLE_MOTOR_ID = 0, + FOLLOWER_ANGLE_MOTOR_ID = 1, + MASTER_ELEVATOR_MOTOR_ID = 2, + FOLLOWER_ELEVATOR_MOTOR_ID = 3, + MAG_ENCODER_ID = 4, + ANGLE_CANCODER_ID = 5; private static final double MASTER_ANGLE_OFFSET_VALUE = 0.0, @@ -39,27 +39,27 @@ public class ArmConstants { private static final SensorDirectionValue CANCODER_INVERTED_VALUE = SensorDirectionValue.Clockwise_Positive; private static final double - P = 1.32, - I = 3.1, - D = 0; - PIDController PID_CONTROLLER = new PIDController(P,I,D); + P = 1.32, + I = 3.1, + D = 0; + PIDController PID_CONTROLLER = new PIDController(P, I, D); private static final CANSparkMax.IdleMode - MASTER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - MASTER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + MASTER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + MASTER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + FOLLOWER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private 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); + 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 TalonSRX MAG_ENCODER = new TalonSRX(MAG_ENCODER_ID); private static final CANcoder CANCODER = new CANcoder(ANGLE_CANCODER_ID); - private static void config_elevator_motors(){ + private static void config_elevator_motors() { MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); @@ -71,7 +71,7 @@ private static void config_elevator_motors(){ } - private static void config_angle_motors(){ + private static void config_angle_motors() { MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); @@ -89,6 +89,7 @@ private static void config_angle_motors(){ config_angle_motors(); config_elevator_motors(); } + public enum ArmStates { FIRST_STATE(43, 1), SECOND_STATE(50, 2), @@ -96,7 +97,7 @@ public enum ArmStates { final double rotation2d; final int elevatorPosition; - ArmStates(double rotation2d, int elevatorPosition){ + ArmStates(double rotation2d, int elevatorPosition) { this.elevatorPosition = elevatorPosition; this.rotation2d = rotation2d; } From 8769ff488683722a8c776007d63d01521e15804d Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 11:32:00 +0200 Subject: [PATCH 04/48] updated ArmConstants --- .../robot/subsystems/arm/ArmConstants.java | 100 ++++++++++-------- 1 file changed, 53 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index fd852d9b3..741e79fe4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -1,17 +1,14 @@ package frc.trigon.robot.subsystems.arm; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.DeviceIdentifier; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; public class ArmConstants { private static final int @@ -19,36 +16,30 @@ public class ArmConstants { FOLLOWER_ANGLE_MOTOR_ID = 1, MASTER_ELEVATOR_MOTOR_ID = 2, FOLLOWER_ELEVATOR_MOTOR_ID = 3, - MAG_ENCODER_ID = 4, + ELEVATOR_MAG_ENCODER_ID = 4, ANGLE_CANCODER_ID = 5; private static final double - MASTER_ANGLE_OFFSET_VALUE = 0.0, - FOLLOWER_ANGLE_OFFSET_VALUE = 0.0, - MASTER_ELEVATOR_OFFSET_VALUE = 0.0, - FOLLOWER_ELEVATOR_OFFSET_VALUE = 0.0, - MAG_ENCODER_OFFSET = 0.0, + ELEVATOR_MAG_ENCODER_OFFSET = 0.0, ANGLE_CANCODER_OFFSET = 0.0; private static final boolean - MASTER_ANGLE_INVERTED_VALUE = true, - FOLLOWER_ANGLE_INVERTED_VALUE = true, + MASTER_ANGLE_INVERTED_VALUE = false, + FOLLOWER_ANGLE_INVERTED_VALUE = false, MASTER_ELEVATOR_INVERTED_VALUE = false, FOLLOWER_ELEVATOR_INVERTED_VALUE = true, - MAG_ENCODER_INVERTED_VALUE = true; - private static final SensorDirectionValue CANCODER_INVERTED_VALUE = SensorDirectionValue.Clockwise_Positive; + ELEVATOR_MAG_ENCODER_PHASE_VALUE = true; + private static final SensorDirectionValue ANGLE_CANCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final double P = 1.32, I = 3.1, D = 0; - PIDController PID_CONTROLLER = new PIDController(P, I, D); + private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); private static final CANSparkMax.IdleMode - MASTER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - MASTER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - FOLLOWER_ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final CANSparkMax @@ -56,50 +47,65 @@ public class ArmConstants { 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 TalonSRX MAG_ENCODER = new TalonSRX(MAG_ENCODER_ID); - private static final CANcoder CANCODER = new CANcoder(ANGLE_CANCODER_ID); + private static final TalonSRX ELEVATOR_MAG_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); + private static final CANcoder ANGLE_CANCODER = new CANcoder(ANGLE_CANCODER_ID); - private static void config_elevator_motors() { - MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); - MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); + static { + motors_angle_config(); + motors_elevator_config(); + configureElevatorEncoder(); + } + + private static void motors_elevator_config() { + MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); - FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE); - MAG_ENCODER.setSelectedSensorPosition(MAG_ENCODER.getSelectedSensorPosition() - MAG_ENCODER_OFFSET); - MAG_ENCODER.setSensorPhase(MAG_ENCODER_INVERTED_VALUE); + MASTER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); + + ELEVATOR_MAG_ENCODER.setSelectedSensorPosition(ELEVATOR_MAG_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); + ELEVATOR_MAG_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE_VALUE); } - private static void config_angle_motors() { - MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); - MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_NEUTRAL_MODE_VALUE); + private static void configureElevatorEncoder() { + ELEVATOR_MAG_ENCODER.setSelectedSensorPosition(ELEVATOR_MAG_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); + ELEVATOR_MAG_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE_VALUE); + } + + private static void motors_angle_config() { + MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + + MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); - FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_NEUTRAL_MODE_VALUE); - CANcoderConfiguration config = new CANcoderConfiguration(); - config.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; - config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; - config.MagnetSensor.SensorDirection = CANCODER_INVERTED_VALUE; - CANCODER.getConfigurator().apply(config); - } + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); - static { - config_angle_motors(); - config_elevator_motors(); + CANcoderConfiguration configureAngleEncoder = new CANcoderConfiguration(); + configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; + configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; + configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_CANCODER_SENSOR_DIRECTION; + ANGLE_CANCODER.getConfigurator().apply(configureAngleEncoder); } + public enum ArmStates { - FIRST_STATE(43, 1), - SECOND_STATE(50, 2), - THIRD_STATE(100, 3); - final double rotation2d; - final int elevatorPosition; + FIRST_STATE(new Rotation2d(30), 1), + SECOND_STATE(new Rotation2d(50), 2), + THIRD_STATE(new Rotation2d(100), 3); + final Rotation2d angle; + + final double elevatorPosition; - ArmStates(double rotation2d, int elevatorPosition) { + ArmStates(Rotation2d angle, double elevatorPosition) { this.elevatorPosition = elevatorPosition; - this.rotation2d = rotation2d; + this.angle = angle; } } From 7a39af8f5b91fefba9584f28457274f534ce65e5 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 11:34:06 +0200 Subject: [PATCH 05/48] updated ArmConstants --- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 741e79fe4..bba1a7b3e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -61,7 +61,7 @@ private static void motors_elevator_config() { FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); - FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED_VALUE); MASTER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); @@ -81,7 +81,7 @@ private static void motors_angle_config() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); - MASTER_ANGLE_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED_VALUE); FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); From 8128bdf0fa745581031156657a967924e5c704ea Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 11:47:22 +0200 Subject: [PATCH 06/48] updated ArmConstants.java --- .../robot/subsystems/arm/ArmConstants.java | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index bba1a7b3e..ba20b84dd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -9,6 +9,7 @@ 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 ArmConstants { private static final int @@ -37,18 +38,27 @@ public class ArmConstants { D = 0; private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + private static final double + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500, + MAX_ELEVATOR_VELOCITY = 900, + MAX_ELEVATOR_ACCELERATION = 550; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); + static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); + + private static final CANSparkMax.IdleMode ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - private static final CANSparkMax + 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 TalonSRX ELEVATOR_MAG_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); - private static final CANcoder ANGLE_CANCODER = new CANcoder(ANGLE_CANCODER_ID); + static final TalonSRX ELEVATOR_MAG_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); + static final CANcoder ANGLE_CANCODER = new CANcoder(ANGLE_CANCODER_ID); static { motors_angle_config(); From 23287643963bf320d06a38df0da55445f7682766 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 13:36:14 +0200 Subject: [PATCH 07/48] Update ArmConstants.java --- .../robot/subsystems/arm/ArmConstants.java | 78 ++++++++++--------- 1 file changed, 40 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index ba20b84dd..fbf0d72e8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -25,19 +25,13 @@ public class ArmConstants { ANGLE_CANCODER_OFFSET = 0.0; private static final boolean - MASTER_ANGLE_INVERTED_VALUE = false, - FOLLOWER_ANGLE_INVERTED_VALUE = false, - MASTER_ELEVATOR_INVERTED_VALUE = false, - FOLLOWER_ELEVATOR_INVERTED_VALUE = true, - ELEVATOR_MAG_ENCODER_PHASE_VALUE = true; + MASTER_ANGLE_INVERTED = false, + FOLLOWER_ANGLE_INVERTED = false, + MASTER_ELEVATOR_INVERTED = false, + FOLLOWER_ELEVATOR_INVERTED = true, + ELEVATOR_MAG_ENCODER_PHASE = true; private static final SensorDirectionValue ANGLE_CANCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; - private static final double - P = 1.32, - I = 3.1, - D = 0; - private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); - private static final double MAX_ANGLE_VELOCITY = 600, MAX_ANGLE_ACCELERATION = 500, @@ -45,11 +39,10 @@ public class ArmConstants { MAX_ELEVATOR_ACCELERATION = 550; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); - - + private static final CANSparkMax.IdleMode - ANGLE_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - ELEVATOR_NEUTRAL_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + ANGLE_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake, + ELEVATOR_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final CANSparkMax @@ -57,60 +50,69 @@ public class ArmConstants { 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); - static final TalonSRX ELEVATOR_MAG_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); - static final CANcoder ANGLE_CANCODER = new CANcoder(ANGLE_CANCODER_ID); + static final TalonSRX ELEVATOR_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_CANCODER_ID); + + private static final double + P = 1.32, + I = 3.1, + D = 0; + private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); static { - motors_angle_config(); - motors_elevator_config(); + configureAngleMotors(); + configureElevatorMotors(); configureElevatorEncoder(); + configureAngleEncoder(); } - private static void motors_elevator_config() { + private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); - MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED_VALUE); - FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED_VALUE); - - MASTER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); - FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); - - ELEVATOR_MAG_ENCODER.setSelectedSensorPosition(ELEVATOR_MAG_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); - ELEVATOR_MAG_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE_VALUE); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); + MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE_VALUE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE_VALUE); } + private static void configureElevatorEncoder() { - ELEVATOR_MAG_ENCODER.setSelectedSensorPosition(ELEVATOR_MAG_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); - ELEVATOR_MAG_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE_VALUE); + ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); + ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE); } - private static void motors_angle_config() { + private static void configureAngleMotors() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); - MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED_VALUE); - FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED_VALUE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); - MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); - FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_NEUTRAL_MODE_VALUE); + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE_VALUE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE_VALUE); + } + + private static void configureAngleEncoder() { CANcoderConfiguration configureAngleEncoder = new CANcoderConfiguration(); configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_CANCODER_SENSOR_DIRECTION; - ANGLE_CANCODER.getConfigurator().apply(configureAngleEncoder); + ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); } - public enum ArmStates { + + + public enum ArmStates { FIRST_STATE(new Rotation2d(30), 1), SECOND_STATE(new Rotation2d(50), 2), THIRD_STATE(new Rotation2d(100), 3); - final Rotation2d angle; + final Rotation2d angle; final double elevatorPosition; ArmStates(Rotation2d angle, double elevatorPosition) { From 28a7ab07b28658e194fda33bf78b2c319497c0ca Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 13:47:47 +0200 Subject: [PATCH 08/48] FIX --- .../robot/subsystems/arm/ArmConstants.java | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index fbf0d72e8..eb53a4244 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -59,6 +59,8 @@ public class ArmConstants { D = 0; private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static { configureAngleMotors(); configureElevatorMotors(); @@ -69,6 +71,10 @@ public class ArmConstants { private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); @@ -79,6 +85,7 @@ private static void configureElevatorMotors() { private static void configureElevatorEncoder() { + ELEVATOR_ENCODER.configFactoryDefault(); ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE); } @@ -88,6 +95,9 @@ private static void configureAngleMotors() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); @@ -95,7 +105,6 @@ private static void configureAngleMotors() { FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE_VALUE); } - private static void configureAngleEncoder() { CANcoderConfiguration configureAngleEncoder = new CANcoderConfiguration(); configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; @@ -104,13 +113,10 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); } - - - public enum ArmStates { - FIRST_STATE(new Rotation2d(30), 1), - SECOND_STATE(new Rotation2d(50), 2), - THIRD_STATE(new Rotation2d(100), 3); + FIRST_STATE(Rotation2d.fromDegrees(30), 1), + SECOND_STATE(Rotation2d.fromDegrees(50), 2), + THIRD_STATE(Rotation2d.fromDegrees(100), 3); final Rotation2d angle; final double elevatorPosition; From 82882a0af382278806b0e55fca668320b4b85f6b Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 13 Nov 2023 13:49:45 +0200 Subject: [PATCH 09/48] FIX --- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index eb53a4244..7fb56beaf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -33,10 +33,10 @@ public class ArmConstants { private static final SensorDirectionValue ANGLE_CANCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final double - MAX_ANGLE_VELOCITY = 600, - MAX_ANGLE_ACCELERATION = 500, - MAX_ELEVATOR_VELOCITY = 900, - MAX_ELEVATOR_ACCELERATION = 550; + MAX_ANGLE_VELOCITY = 100, + MAX_ANGLE_ACCELERATION = 100, + MAX_ELEVATOR_VELOCITY = 100, + MAX_ELEVATOR_ACCELERATION = 100; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); @@ -71,7 +71,7 @@ public class ArmConstants { private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); - + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); From 99805bbd9fed955422a9a669f6d9af1ea09847e4 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Tue, 14 Nov 2023 17:31:41 +0200 Subject: [PATCH 10/48] changed the names --- .../robot/subsystems/arm/ArmConstants.java | 53 ++++++++++--------- 1 file changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 7fb56beaf..2ba08ae96 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -12,34 +12,36 @@ 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, - ELEVATOR_MAG_ENCODER_ID = 4, - ANGLE_CANCODER_ID = 5; + ELEVATOR_ENCODER_ID = 4, + ANGLE_ENCODER_ID = 5; private static final double - ELEVATOR_MAG_ENCODER_OFFSET = 0.0, - ANGLE_CANCODER_OFFSET = 0.0; + ELEVATOR_ENCODER_OFFSET = 0.0, + ANGLE_ENCODER_OFFSET = 0.0; private static final boolean MASTER_ANGLE_INVERTED = false, FOLLOWER_ANGLE_INVERTED = false, MASTER_ELEVATOR_INVERTED = false, FOLLOWER_ELEVATOR_INVERTED = true, - ELEVATOR_MAG_ENCODER_PHASE = true; - private static final SensorDirectionValue ANGLE_CANCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; + ELEVATOR_ENCODER_PHASE = true; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; private static final double - MAX_ANGLE_VELOCITY = 100, - MAX_ANGLE_ACCELERATION = 100, - MAX_ELEVATOR_VELOCITY = 100, - MAX_ELEVATOR_ACCELERATION = 100; + MAX_ANGLE_VELOCITY = 100, + MAX_ANGLE_ACCELERATION = 100, + MAX_ELEVATOR_VELOCITY = 100, + MAX_ELEVATOR_ACCELERATION = 100; static final TrapezoidProfile.Constraints ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); - + private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake, ELEVATOR_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake; @@ -50,16 +52,19 @@ public class ArmConstants { 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); - static final TalonSRX ELEVATOR_ENCODER = new TalonSRX(ELEVATOR_MAG_ENCODER_ID); - static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_CANCODER_ID); + static final TalonSRX ELEVATOR_ENCODER = new TalonSRX(ELEVATOR_ENCODER_ID); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); private static final double - P = 1.32, - I = 3.1, - D = 0; - private static final PIDController PID_CONTROLLER = new PIDController(P, I, D); - - private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + ELEVATOR_P = 1.32, + ELEVATOR_I = 3.1, + ELEVATOR_D = 0, + ANGLE_P = 1.32, + ANGLE_I = 3.1, + ANGLE_D = 0; + private static final PIDController + ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D), + ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D); static { configureAngleMotors(); @@ -86,8 +91,8 @@ private static void configureElevatorMotors() { private static void configureElevatorEncoder() { ELEVATOR_ENCODER.configFactoryDefault(); - ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_MAG_ENCODER_OFFSET); - ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_MAG_ENCODER_PHASE); + ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET); + ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); } @@ -107,13 +112,13 @@ private static void configureAngleMotors() { private static void configureAngleEncoder() { CANcoderConfiguration configureAngleEncoder = new CANcoderConfiguration(); - configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_CANCODER_OFFSET; + configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; - configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_CANCODER_SENSOR_DIRECTION; + configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); } - public enum ArmStates { + public enum ArmStates { FIRST_STATE(Rotation2d.fromDegrees(30), 1), SECOND_STATE(Rotation2d.fromDegrees(50), 2), THIRD_STATE(Rotation2d.fromDegrees(100), 3); From 773f4f34cfc837dda32b43c8ec91777b9105ee2f Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Tue, 14 Nov 2023 19:30:03 +0200 Subject: [PATCH 11/48] ___FIXED___ --- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 2ba08ae96..735433571 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.arm; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; @@ -39,8 +40,9 @@ public class ArmConstants { MAX_ANGLE_ACCELERATION = 100, MAX_ELEVATOR_VELOCITY = 100, MAX_ELEVATOR_ACCELERATION = 100; - static final TrapezoidProfile.Constraints ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); - static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); + static final TrapezoidProfile.Constraints + ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), + ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake, @@ -80,7 +82,6 @@ private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); - MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); @@ -88,14 +89,13 @@ private static void configureElevatorMotors() { FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE_VALUE); } - private static void configureElevatorEncoder() { + ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); ELEVATOR_ENCODER.configFactoryDefault(); ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET); ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); } - private static void configureAngleMotors() { MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); From 113172a0f8c8e5156b97933b3146c62799cedd64 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Tue, 14 Nov 2023 19:39:48 +0200 Subject: [PATCH 12/48] Update ArmConstants.java --- .../robot/subsystems/arm/ArmConstants.java | 28 ++++++++++++++----- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 735433571..a4be8da77 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -8,6 +8,8 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -44,9 +46,21 @@ public class ArmConstants { ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0, + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS,ANGLE_MOTOR_KG,ANGLE_MOTOR_KV,ANGLE_MOTOR_KA); + static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward(ELEVATOR_MOTOR_KS,ELEVATOR_MOTOR_KG,ELEVATOR_MOTOR_KV,ELEVATOR_MOTOR_KA); + private static final CANSparkMax.IdleMode - ANGLE_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake, - ELEVATOR_IDLE_MODE_VALUE = CANSparkMax.IdleMode.kBrake; + ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final CANSparkMax @@ -85,13 +99,13 @@ private static void configureElevatorMotors() { MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); - MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE_VALUE); - FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE_VALUE); + MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); } private static void configureElevatorEncoder() { - ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); ELEVATOR_ENCODER.configFactoryDefault(); + ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET); ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); } @@ -106,8 +120,8 @@ private static void configureAngleMotors() { MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); - MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE_VALUE); - FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE_VALUE); + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); } private static void configureAngleEncoder() { From cb29753b50a7666c1e0de35e951029aef6df4e5a Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Wed, 15 Nov 2023 09:06:12 +0200 Subject: [PATCH 13/48] Update ArmConstants.java --- .../robot/subsystems/arm/ArmConstants.java | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index a4be8da77..6813fb6e8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -37,27 +37,6 @@ public class ArmConstants { ELEVATOR_ENCODER_PHASE = true; private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive; - private static final double - MAX_ANGLE_VELOCITY = 100, - MAX_ANGLE_ACCELERATION = 100, - MAX_ELEVATOR_VELOCITY = 100, - MAX_ELEVATOR_ACCELERATION = 100; - static final TrapezoidProfile.Constraints - ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), - ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); - - private static final double - ANGLE_MOTOR_KS = 0, - ANGLE_MOTOR_KV = 0, - ANGLE_MOTOR_KA = 0, - ANGLE_MOTOR_KG = 0, - ELEVATOR_MOTOR_KS = 0, - ELEVATOR_MOTOR_KV = 0, - ELEVATOR_MOTOR_KA = 0, - ELEVATOR_MOTOR_KG = 0; - static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS,ANGLE_MOTOR_KG,ANGLE_MOTOR_KV,ANGLE_MOTOR_KA); - static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward(ELEVATOR_MOTOR_KS,ELEVATOR_MOTOR_KG,ELEVATOR_MOTOR_KV,ELEVATOR_MOTOR_KA); - private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; @@ -82,6 +61,28 @@ public class ArmConstants { ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D), ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D); + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0, + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA); + static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA); + + private static final double + MAX_ANGLE_VELOCITY = 100, + MAX_ANGLE_ACCELERATION = 100, + MAX_ELEVATOR_VELOCITY = 100, + MAX_ELEVATOR_ACCELERATION = 100; + static final TrapezoidProfile.Constraints + ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), + ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); + + static { configureAngleMotors(); configureElevatorMotors(); From b5f06cbac84723a944635db29e62da0f86d98c89 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Wed, 15 Nov 2023 12:17:17 +0200 Subject: [PATCH 14/48] Update ArmConstants.java --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 6813fb6e8..6c8586475 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -82,7 +82,6 @@ public class ArmConstants { ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); - static { configureAngleMotors(); configureElevatorMotors(); From b10639efd7e428dd20171960c2e7892144890d37 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 09:07:19 +0200 Subject: [PATCH 15/48] Create ArmSubsystem.java --- .../robot/subsystems/arm/ArmSubsystem.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java new file mode 100644 index 000000000..d0e73a62c --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java @@ -0,0 +1,17 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ArmSubsystem extends SubsystemBase { + + private final static ArmSubsystem INSTANCE = new ArmSubsystem(); + + @SuppressWarnings("WeakerAccess") + public static ArmSubsystem getInstance() { + return INSTANCE; + } + + private ArmSubsystem() { + } +} + From 75998fa082e294604c2c820ad156994956958c13 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 09:07:41 +0200 Subject: [PATCH 16/48] Update ArmConstants.java --- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 6c8586475..3b8d90189 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -132,7 +132,7 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); } - public enum ArmStates { + public enum ArmState { FIRST_STATE(Rotation2d.fromDegrees(30), 1), SECOND_STATE(Rotation2d.fromDegrees(50), 2), THIRD_STATE(Rotation2d.fromDegrees(100), 3); @@ -140,7 +140,7 @@ public enum ArmStates { final Rotation2d angle; final double elevatorPosition; - ArmStates(Rotation2d angle, double elevatorPosition) { + ArmState(Rotation2d angle, double elevatorPosition) { this.elevatorPosition = elevatorPosition; this.angle = angle; } From 50fbce08c703bc28ce5b94b3b495b84eb476e78b Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 09:12:32 +0200 Subject: [PATCH 17/48] ____added___motors___ --- .../trigon/robot/subsystems/arm/ArmSubsystem.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java index d0e73a62c..b288a97b3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java @@ -1,11 +1,21 @@ package frc.trigon.robot.subsystems.arm; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ArmSubsystem extends SubsystemBase { + static final CANSparkMax + masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR, + followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR, + masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + static final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; + static final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; private final static ArmSubsystem INSTANCE = new ArmSubsystem(); - @SuppressWarnings("WeakerAccess") public static ArmSubsystem getInstance() { return INSTANCE; From b75612e7f4f5b7bd519fd82a9bdfc8259251e7ec Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 09:14:23 +0200 Subject: [PATCH 18/48] made it static --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 3b8d90189..15fec5d77 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -57,7 +57,7 @@ public class ArmConstants { ANGLE_P = 1.32, ANGLE_I = 3.1, ANGLE_D = 0; - private static final PIDController + static final PIDController ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D), ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D); From 32bb47730b4a66c735e05daf8b760110a8a49f05 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 09:19:20 +0200 Subject: [PATCH 19/48] Update ArmSubsystem.java --- .../robot/subsystems/arm/ArmSubsystem.java | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java index b288a97b3..7c9f8d8f3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java @@ -4,16 +4,26 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ArmSubsystem extends SubsystemBase { - static final CANSparkMax + private static final CANSparkMax masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR, followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR, masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; - static final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; - static final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; + private static final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; + private static final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; + + private static final ArmFeedforward angleMotorFeedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD; + private static final ElevatorFeedforward elevatorMotorFeedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; + + private static final TrapezoidProfile.Constraints + angleConstrains = ArmConstants.ANGLE_CONSTRAINS, + elevatorConstrains = ArmConstants.ELEVATOR_CONSTRAINS; private final static ArmSubsystem INSTANCE = new ArmSubsystem(); @SuppressWarnings("WeakerAccess") From ca981c8137c2b0a6fafa3796d9419b2ae195f0f9 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 19:51:37 +0200 Subject: [PATCH 20/48] ==!!!___updated___!!!== --- ...Build & Deploy Robot for Debugging.run.xml | 22 +++++++++++++++ .run/Build & Deploy Robot.run.xml | 22 +++++++++++++++ .run/Build Robot.run.xml | 22 +++++++++++++++ ...Build & Deploy Robot for Debugging.run.xml | 23 +++++++++++++++ .run/Clean Build & Deploy Robot.run.xml | 23 +++++++++++++++ .run/Clean Build Robot.run.xml | 23 +++++++++++++++ .run/Clean.run.xml | 22 +++++++++++++++ .../frc/trigon/robot/subsystems/arm/Arm.java | 28 +++++++++++++++++++ 8 files changed, 185 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/Arm.java diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml index a1f77b460..72b01bfb5 100644 --- a/.run/Build & Deploy Robot for Debugging.run.xml +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -43,4 +43,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml index cbc14915c..0f8a7ebf4 100644 --- a/.run/Build & Deploy Robot.run.xml +++ b/.run/Build & Deploy Robot.run.xml @@ -43,4 +43,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index 600f8703a..d78b6e76e 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -43,4 +43,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml index c3fbaf027..13d0e1acf 100644 --- a/.run/Clean Build & Deploy Robot for Debugging.run.xml +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -45,4 +45,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml index 5283e7aa2..209136095 100644 --- a/.run/Clean Build & Deploy Robot.run.xml +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -45,4 +45,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml index 2f5509e97..8f2b4e867 100644 --- a/.run/Clean Build Robot.run.xml +++ b/.run/Clean Build Robot.run.xml @@ -45,4 +45,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml index ac04edd82..20d02cbf5 100644 --- a/.run/Clean.run.xml +++ b/.run/Clean.run.xml @@ -43,4 +43,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file 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..f00272899 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,28 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + private final static Arm INSTANCE = new Arm(); + + public static Arm getInstance() {return INSTANCE;} + + private final CANSparkMax + masterAngleMotor = frc.trigon.robot.subsystems.arm.ArmConstants.MASTER_ANGLE_MOTOR, + followerAngleMotor = frc.trigon.robot.subsystems.arm.ArmConstants.FOLLOWER_ANGLE_MOTOR, + masterElevatorMotor = frc.trigon.robot.subsystems.arm.ArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = frc.trigon.robot.subsystems.arm.ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final TalonSRX elevatorEncoder = frc.trigon.robot.subsystems.arm.ArmConstants.ELEVATOR_ENCODER; + private final CANcoder angleEncoder = frc.trigon.robot.subsystems.arm.ArmConstants.ANGLE_ENCODER; + + private final ArmFeedforward angleMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ANGLE_MOTOR_FEEDFORWARD; + private final ElevatorFeedforward elevatorMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; + + private Arm() { + } +} From 2385256b812de6b0b29c09945dc3df3461c71324 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 19:56:05 +0200 Subject: [PATCH 21/48] i dont know what it is --- .gitignore | 2 + .../robot/subsystems/arm/ArmSubsystem.java | 37 ------------------- 2 files changed, 2 insertions(+), 37 deletions(-) delete mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java diff --git a/.gitignore b/.gitignore index 9aed7532d..94dca35ec 100644 --- a/.gitignore +++ b/.gitignore @@ -331,3 +331,5 @@ bin/ # End entries from WPI Lib team +src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java +src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java deleted file mode 100644 index 7c9f8d8f3..000000000 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.trigon.robot.subsystems.arm; - -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ArmSubsystem extends SubsystemBase { - private static final CANSparkMax - masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR, - followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR, - masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, - followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; - private static final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; - private static final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; - - private static final ArmFeedforward angleMotorFeedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD; - private static final ElevatorFeedforward elevatorMotorFeedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; - - private static final TrapezoidProfile.Constraints - angleConstrains = ArmConstants.ANGLE_CONSTRAINS, - elevatorConstrains = ArmConstants.ELEVATOR_CONSTRAINS; - - private final static ArmSubsystem INSTANCE = new ArmSubsystem(); - @SuppressWarnings("WeakerAccess") - public static ArmSubsystem getInstance() { - return INSTANCE; - } - - private ArmSubsystem() { - } -} - From 1dcd1e40f4f55dedf3d4232d291185815bf2f3b0 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 16 Nov 2023 22:46:31 +0200 Subject: [PATCH 22/48] added angle methods --- .../frc/trigon/robot/subsystems/arm/Arm.java | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) 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 f00272899..822683e9b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -5,6 +5,11 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -23,6 +28,50 @@ public class Arm extends SubsystemBase { private final ArmFeedforward angleMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ANGLE_MOTOR_FEEDFORWARD; private final ElevatorFeedforward elevatorMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; + private TrapezoidProfile + angleMotorProfile = null, + elevatorMotorProfile = null; + private double + lastAngleProfileGeneration, + lastElevatorProfileGeneration; + private double getAngleMotorPositionDegrees(){ + return angleEncoder.getPosition().getValue(); + } + private double getAngleMotorVelocity(){ + return masterAngleMotor.getVoltageCompensationNominalVoltage(); + } + + private void generateAngleMotorProfile(Rotation2d targetAngle){ + angleMotorProfile = new TrapezoidProfile( + ArmConstants.ANGLE_CONSTRAINS, + new TrapezoidProfile.State(targetAngle.getDegrees(),0), + new TrapezoidProfile.State(getAngleMotorPositionDegrees(), getAngleMotorVelocity()) + ); + lastAngleProfileGeneration = Timer.getFPGATimestamp(); + } + + private double getAngleMotorProfileTime(){ + return Timer.getFPGATimestamp() - lastAngleProfileGeneration; + } + + private void setTargetAngleFromProfile(){ + if (angleMotorProfile == null){ + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); + return; + } + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + double pidOutPut = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleMotorPositionDegrees(), + targetState.position + ); + double feedForward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + masterAngleMotor.setVoltage(pidOutPut + feedForward); + } + private Arm() { } } From 1372f92774f79bf2773749ffdde5f7823e0414bb Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Fri, 17 Nov 2023 12:00:40 +0200 Subject: [PATCH 23/48] I added the profile of the elevator --- .../frc/trigon/robot/subsystems/arm/Arm.java | 91 ++++++++++++++----- 1 file changed, 68 insertions(+), 23 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 822683e9b..dfb682ee3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -15,18 +15,16 @@ public class Arm extends SubsystemBase { private final static Arm INSTANCE = new Arm(); - public static Arm getInstance() {return INSTANCE;} - private final CANSparkMax - masterAngleMotor = frc.trigon.robot.subsystems.arm.ArmConstants.MASTER_ANGLE_MOTOR, - followerAngleMotor = frc.trigon.robot.subsystems.arm.ArmConstants.FOLLOWER_ANGLE_MOTOR, - masterElevatorMotor = frc.trigon.robot.subsystems.arm.ArmConstants.MASTER_ELEVATOR_MOTOR, - followerElevatorMotor = frc.trigon.robot.subsystems.arm.ArmConstants.FOLLOWER_ELEVATOR_MOTOR; - private final TalonSRX elevatorEncoder = frc.trigon.robot.subsystems.arm.ArmConstants.ELEVATOR_ENCODER; - private final CANcoder angleEncoder = frc.trigon.robot.subsystems.arm.ArmConstants.ANGLE_ENCODER; + masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR, + followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR, + masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; + private final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; - private final ArmFeedforward angleMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ANGLE_MOTOR_FEEDFORWARD; - private final ElevatorFeedforward elevatorMotorFeedforward = frc.trigon.robot.subsystems.arm.ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; + private final ArmFeedforward angleMotorFeedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD; + private final ElevatorFeedforward elevatorMotorFeedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; private TrapezoidProfile angleMotorProfile = null, @@ -34,34 +32,61 @@ public class Arm extends SubsystemBase { private double lastAngleProfileGeneration, lastElevatorProfileGeneration; - private double getAngleMotorPositionDegrees(){ + + public static Arm getInstance() { + return INSTANCE; + } + + private double getAngleMotorPositionDegrees() { return angleEncoder.getPosition().getValue(); } - private double getAngleMotorVelocity(){ - return masterAngleMotor.getVoltageCompensationNominalVoltage(); + + private double getElevatorMotorPositionDegrees() { + return elevatorEncoder.getSelectedSensorPosition(); + } + + private double getAngleMotorVelocity() { + return angleEncoder.getVelocity().getValue(); + } + + private double getElevatorMotorVelocity() { + return elevatorEncoder.getSelectedSensorVelocity(); + } + + private double getAngleMotorProfileTime() { + return Timer.getFPGATimestamp() - lastAngleProfileGeneration; + } + + private double getElevatorMotorProfileTime() { + return Timer.getFPGATimestamp() - lastElevatorProfileGeneration; } - private void generateAngleMotorProfile(Rotation2d targetAngle){ + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, - new TrapezoidProfile.State(targetAngle.getDegrees(),0), - new TrapezoidProfile.State(getAngleMotorPositionDegrees(), getAngleMotorVelocity()) + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAngleMotorPositionDegrees(), getElevatorMotorVelocity()) ); - lastAngleProfileGeneration = Timer.getFPGATimestamp(); + lastElevatorProfileGeneration = Timer.getFPGATimestamp(); } - private double getAngleMotorProfileTime(){ - return Timer.getFPGATimestamp() - lastAngleProfileGeneration; + private void generateElevatorMotorProfile(Rotation2d targetAngle) { + elevatorMotorProfile = new TrapezoidProfile( + ArmConstants.ELEVATOR_CONSTRAINS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getElevatorMotorPositionDegrees(), getAngleMotorVelocity()) + ); + lastAngleProfileGeneration = Timer.getFPGATimestamp(); } - private void setTargetAngleFromProfile(){ - if (angleMotorProfile == null){ + private void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { masterAngleMotor.stopMotor(); followerAngleMotor.stopMotor(); return; } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - double pidOutPut = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( getAngleMotorPositionDegrees(), targetState.position ); @@ -69,7 +94,27 @@ private void setTargetAngleFromProfile(){ Units.degreesToRadians(targetState.position), Units.degreesToRadians(targetState.velocity) ); - masterAngleMotor.setVoltage(pidOutPut + feedForward); + masterAngleMotor.setVoltage(pidOutput + feedForward); + followerAngleMotor.setVoltage(pidOutput + feedForward); + } + + private void setTargetElevatorFromProfile() { + if (elevatorMotorProfile == null) { + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); + return; + } + TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorMotorPositionDegrees(), + targetState.position + ); + double feedForward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + masterElevatorMotor.setVoltage(pidOutput + feedForward); + followerElevatorMotor.setVoltage(pidOutput + feedForward); } private Arm() { From 656b3d8b1e648b3e861ce490f165f487818337c7 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Fri, 17 Nov 2023 12:08:32 +0200 Subject: [PATCH 24/48] corrected the last profile generate --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 dfb682ee3..1c5751735 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -67,7 +67,7 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAngleMotorPositionDegrees(), getElevatorMotorVelocity()) ); - lastElevatorProfileGeneration = Timer.getFPGATimestamp(); + lastAngleProfileGeneration = Timer.getFPGATimestamp(); } private void generateElevatorMotorProfile(Rotation2d targetAngle) { @@ -76,7 +76,7 @@ private void generateElevatorMotorProfile(Rotation2d targetAngle) { new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getElevatorMotorPositionDegrees(), getAngleMotorVelocity()) ); - lastAngleProfileGeneration = Timer.getFPGATimestamp(); + lastElevatorProfileGeneration = Timer.getFPGATimestamp(); } private void setTargetAngleFromProfile() { From 2227ae3514b38af07bc772e5e85cdbbc56d9b00a Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 11:36:14 +0200 Subject: [PATCH 25/48] added and fixed things --- .../frc/trigon/robot/subsystems/arm/Arm.java | 104 ++++++++++-------- .../robot/subsystems/arm/ArmConstants.java | 9 ++ 2 files changed, 70 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 1c5751735..941e03df2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -3,14 +3,12 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; 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 Arm extends SubsystemBase { private final static Arm INSTANCE = new Arm(); @@ -23,9 +21,6 @@ public class Arm extends SubsystemBase { private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; private final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; - private final ArmFeedforward angleMotorFeedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD; - private final ElevatorFeedforward elevatorMotorFeedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD; - private TrapezoidProfile angleMotorProfile = null, elevatorMotorProfile = null; @@ -37,20 +32,27 @@ public static Arm getInstance() { return INSTANCE; } - private double getAngleMotorPositionDegrees() { - return angleEncoder.getPosition().getValue(); + private Arm() { } - private double getElevatorMotorPositionDegrees() { - return elevatorEncoder.getSelectedSensorPosition(); + private void stopAngleMotors(){ + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); } - private double getAngleMotorVelocity() { - return angleEncoder.getVelocity().getValue(); + private void stopElevatorMotors(){ + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); } - private double getElevatorMotorVelocity() { - return elevatorEncoder.getSelectedSensorVelocity(); + private void setAngleMotorsVoltage(double voltage) { + masterAngleMotor.setVoltage(voltage); + followerAngleMotor.setVoltage(voltage); + } + + private void setElevatorMotorsVoltage(double voltage) { + masterElevatorMotor.setVoltage(voltage); + followerElevatorMotor.setVoltage(voltage); } private double getAngleMotorProfileTime() { @@ -61,62 +63,78 @@ private double getElevatorMotorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorProfileGeneration; } + private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleMotorPositionDegrees().getDegrees(), + targetState.position + ); + double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return feedforward + pidOutput; + } + private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorMotorPositionConversions().getDegrees(), + targetState.position + ); + double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); + return feedforward + pidOutput; + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorPositionDegrees(), getElevatorMotorVelocity()) + new TrapezoidProfile.State(getAngleMotorPositionDegrees().getDegrees(), getAngleMotorVelocityPerSecond()) ); lastAngleProfileGeneration = Timer.getFPGATimestamp(); } - private void generateElevatorMotorProfile(Rotation2d targetAngle) { + private void generateElevatorMotorProfile(double position) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, - new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getElevatorMotorPositionDegrees(), getAngleMotorVelocity()) + new TrapezoidProfile.State(position, 0), + new TrapezoidProfile.State(getElevatorMotorPositionConversions().getDegrees(), getAngleMotorVelocityPerSecond()) ); lastElevatorProfileGeneration = Timer.getFPGATimestamp(); } private void setTargetAngleFromProfile() { if (angleMotorProfile == null) { - masterAngleMotor.stopMotor(); - followerAngleMotor.stopMotor(); + stopAngleMotors(); return; } TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); - double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorPositionDegrees(), - targetState.position - ); - double feedForward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( - Units.degreesToRadians(targetState.position), - Units.degreesToRadians(targetState.velocity) - ); - masterAngleMotor.setVoltage(pidOutput + feedForward); - followerAngleMotor.setVoltage(pidOutput + feedForward); + setAngleMotorsVoltage(calculateAngleMotorOutput(targetState)); } private void setTargetElevatorFromProfile() { if (elevatorMotorProfile == null) { - masterElevatorMotor.stopMotor(); - followerElevatorMotor.stopMotor(); + stopElevatorMotors(); return; } TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); - double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getElevatorMotorPositionDegrees(), - targetState.position - ); - double feedForward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate( - Units.degreesToRadians(targetState.position), - Units.degreesToRadians(targetState.velocity) - ); - masterElevatorMotor.setVoltage(pidOutput + feedForward); - followerElevatorMotor.setVoltage(pidOutput + feedForward); + setElevatorMotorsVoltage(calculateElevatorMotorOutput(targetState)); + } + + private Rotation2d getAngleMotorPositionDegrees() { + double position = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); + return new Rotation2d(position); } - private Arm() { + private Rotation2d getElevatorMotorPositionConversions() { + return new Rotation2d(elevatorEncoder.getSelectedSensorPosition()); + } + + private double getAngleMotorVelocityPerSecond() { + double velocity = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); + return Conversions.revolutionsToDegrees(velocity); + } + + private double getElevatorMotorVelocityUnit() { + double velocity = elevatorEncoder.getSelectedSensorVelocity(); + return Conversions.degreesToRevolutions(velocity); } } 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 15fec5d77..f476d7f6c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -2,6 +2,7 @@ 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; @@ -82,6 +83,11 @@ public class ArmConstants { ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); + + static final StatusSignal + ANGLE_MOTOR_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_MOTOR_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + static { configureAngleMotors(); configureElevatorMotors(); @@ -130,6 +136,9 @@ private static void configureAngleEncoder() { configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); + ANGLE_MOTOR_POSITION_SIGNAL.setUpdateFrequency(111); + ANGLE_MOTOR_VELOCITY_SIGNAL.setUpdateFrequency(111); + ANGLE_ENCODER.optimizeBusUtilization(); } public enum ArmState { From f8d3f49286386187a9e69037dff406b3c0ae120e Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:17:53 +0200 Subject: [PATCH 26/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 82 +++++++++---------- 1 file changed, 41 insertions(+), 41 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 941e03df2..6b9974d03 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -25,8 +25,8 @@ public class Arm extends SubsystemBase { angleMotorProfile = null, elevatorMotorProfile = null; private double - lastAngleProfileGeneration, - lastElevatorProfileGeneration; + lastAngleProfileGenerationTime, + lastElevatorProfileGenerationTime; public static Arm getInstance() { return INSTANCE; @@ -35,34 +35,6 @@ public static Arm getInstance() { private Arm() { } - private void stopAngleMotors(){ - masterAngleMotor.stopMotor(); - followerAngleMotor.stopMotor(); - } - - private void stopElevatorMotors(){ - masterElevatorMotor.stopMotor(); - followerElevatorMotor.stopMotor(); - } - - private void setAngleMotorsVoltage(double voltage) { - masterAngleMotor.setVoltage(voltage); - followerAngleMotor.setVoltage(voltage); - } - - private void setElevatorMotorsVoltage(double voltage) { - masterElevatorMotor.setVoltage(voltage); - followerElevatorMotor.setVoltage(voltage); - } - - private double getAngleMotorProfileTime() { - return Timer.getFPGATimestamp() - lastAngleProfileGeneration; - } - - private double getElevatorMotorProfileTime() { - return Timer.getFPGATimestamp() - lastElevatorProfileGeneration; - } - private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( getAngleMotorPositionDegrees().getDegrees(), @@ -76,7 +48,7 @@ private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ } private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getElevatorMotorPositionConversions().getDegrees(), + getElevatorMotorPositionDegrees().getDegrees(), targetState.position ); double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); @@ -89,16 +61,16 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { new TrapezoidProfile.State(targetAngle.getDegrees(), 0), new TrapezoidProfile.State(getAngleMotorPositionDegrees().getDegrees(), getAngleMotorVelocityPerSecond()) ); - lastAngleProfileGeneration = Timer.getFPGATimestamp(); + lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } - private void generateElevatorMotorProfile(double position) { + private void generateElevatorMotorProfile(double targetPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, - new TrapezoidProfile.State(position, 0), - new TrapezoidProfile.State(getElevatorMotorPositionConversions().getDegrees(), getAngleMotorVelocityPerSecond()) + new TrapezoidProfile.State(targetPosition, 0), + new TrapezoidProfile.State(getElevatorMotorPositionDegrees().getDegrees(), getElevatorMotorVelocityRevolutionsPerSeconds()) ); - lastElevatorProfileGeneration = Timer.getFPGATimestamp(); + lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); } private void setTargetAngleFromProfile() { @@ -118,23 +90,51 @@ private void setTargetElevatorFromProfile() { TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); setElevatorMotorsVoltage(calculateElevatorMotorOutput(targetState)); } - + + private double getAngleMotorProfileTime() { + return Timer.getFPGATimestamp() - lastAngleProfileGenerationTime; + } + + private double getElevatorMotorProfileTime() { + return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime; + } + private Rotation2d getAngleMotorPositionDegrees() { double position = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); return new Rotation2d(position); } - private Rotation2d getElevatorMotorPositionConversions() { + private Rotation2d getElevatorMotorPositionDegrees() { return new Rotation2d(elevatorEncoder.getSelectedSensorPosition()); } private double getAngleMotorVelocityPerSecond() { - double velocity = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); - return Conversions.revolutionsToDegrees(velocity); + double position = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); + return Conversions.revolutionsToDegrees(position); } - private double getElevatorMotorVelocityUnit() { + private double getElevatorMotorVelocityRevolutionsPerSeconds() { double velocity = elevatorEncoder.getSelectedSensorVelocity(); return Conversions.degreesToRevolutions(velocity); } + + private void stopAngleMotors(){ + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); + } + + private void stopElevatorMotors(){ + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); + } + + private void setAngleMotorsVoltage(double voltage) { + masterAngleMotor.setVoltage(voltage); + followerAngleMotor.setVoltage(voltage); + } + + private void setElevatorMotorsVoltage(double voltage) { + masterElevatorMotor.setVoltage(voltage); + followerElevatorMotor.setVoltage(voltage); + } } From aa1834543431d8871a9eb4e8e68aac9f5ad2dfc2 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:27:09 +0200 Subject: [PATCH 27/48] Fixed .run folder --- .run/Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Build & Deploy Robot.run.xml | 6 +++--- .run/Build Robot.run.xml | 6 +++--- .run/Clean Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Clean Build & Deploy Robot.run.xml | 6 +++--- .run/Clean Build Robot.run.xml | 6 +++--- .run/Clean.run.xml | 6 +++--- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml index 72b01bfb5..821ba32df 100644 --- a/.run/Build & Deploy Robot for Debugging.run.xml +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml index 0f8a7ebf4..3d60b7bd7 100644 --- a/.run/Build & Deploy Robot.run.xml +++ b/.run/Build & Deploy Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index d78b6e76e..3899ab5c5 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml index 13d0e1acf..21d486c52 100644 --- a/.run/Clean Build & Deploy Robot for Debugging.run.xml +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml index 209136095..f1b07cd4e 100644 --- a/.run/Clean Build & Deploy Robot.run.xml +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml index 8f2b4e867..7003ca64d 100644 --- a/.run/Clean Build Robot.run.xml +++ b/.run/Clean Build Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml index 20d02cbf5..c6b522475 100644 --- a/.run/Clean.run.xml +++ b/.run/Clean.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file From 41faad41f367d60643f0c41c2e963af61f503b53 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:27:59 +0200 Subject: [PATCH 28/48] Fixed the .gitignore file --- .gitignore | 2 -- 1 file changed, 2 deletions(-) diff --git a/.gitignore b/.gitignore index 94dca35ec..9aed7532d 100644 --- a/.gitignore +++ b/.gitignore @@ -331,5 +331,3 @@ bin/ # End entries from WPI Lib team -src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java -src/main/java/frc/trigon/robot/subsystems/arm/ArmSubsystem.java From 097d24d3b1b27ffeacf4afa8d1c74b56b804c899 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:30:14 +0200 Subject: [PATCH 29/48] Revert "Fixed .run folder" This reverts commit aa1834543431d8871a9eb4e8e68aac9f5ad2dfc2. --- .run/Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Build & Deploy Robot.run.xml | 6 +++--- .run/Build Robot.run.xml | 6 +++--- .run/Clean Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Clean Build & Deploy Robot.run.xml | 6 +++--- .run/Clean Build Robot.run.xml | 6 +++--- .run/Clean.run.xml | 6 +++--- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml index 821ba32df..72b01bfb5 100644 --- a/.run/Build & Deploy Robot for Debugging.run.xml +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml index 3d60b7bd7..0f8a7ebf4 100644 --- a/.run/Build & Deploy Robot.run.xml +++ b/.run/Build & Deploy Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index 3899ab5c5..d78b6e76e 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml index 21d486c52..13d0e1acf 100644 --- a/.run/Clean Build & Deploy Robot for Debugging.run.xml +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml index f1b07cd4e..209136095 100644 --- a/.run/Clean Build & Deploy Robot.run.xml +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml index 7003ca64d..8f2b4e867 100644 --- a/.run/Clean Build Robot.run.xml +++ b/.run/Clean Build Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml index c6b522475..20d02cbf5 100644 --- a/.run/Clean.run.xml +++ b/.run/Clean.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file From fc7840a2803fc7d931729e3d6edcd85c16c3e5f4 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:32:22 +0200 Subject: [PATCH 30/48] Fixed the .run folder --- .run/Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Build & Deploy Robot.run.xml | 6 +++--- .run/Build Robot.run.xml | 6 +++--- .run/Clean Build & Deploy Robot for Debugging.run.xml | 6 +++--- .run/Clean Build & Deploy Robot.run.xml | 6 +++--- .run/Clean Build Robot.run.xml | 6 +++--- .run/Clean.run.xml | 6 +++--- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml index 72b01bfb5..821ba32df 100644 --- a/.run/Build & Deploy Robot for Debugging.run.xml +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml index 0f8a7ebf4..3d60b7bd7 100644 --- a/.run/Build & Deploy Robot.run.xml +++ b/.run/Build & Deploy Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index d78b6e76e..3899ab5c5 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml index 13d0e1acf..21d486c52 100644 --- a/.run/Clean Build & Deploy Robot for Debugging.run.xml +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml index 209136095..f1b07cd4e 100644 --- a/.run/Clean Build & Deploy Robot.run.xml +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml index 8f2b4e867..7003ca64d 100644 --- a/.run/Clean Build Robot.run.xml +++ b/.run/Clean Build Robot.run.xml @@ -19,7 +19,7 @@ true true false - false + false @@ -42,7 +42,7 @@ true true false - false + false @@ -65,7 +65,7 @@ true true false - false + false \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml index 20d02cbf5..c6b522475 100644 --- a/.run/Clean.run.xml +++ b/.run/Clean.run.xml @@ -18,7 +18,7 @@ true true false - false + false @@ -40,7 +40,7 @@ true true false - false + false @@ -62,7 +62,7 @@ true true false - false + false \ No newline at end of file From bb4e12b05907ed600db43ed859331c04b018485a Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:35:43 +0200 Subject: [PATCH 31/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 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 6b9974d03..f723f067c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -37,7 +37,7 @@ private Arm() { private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( - getAngleMotorPositionDegrees().getDegrees(), + getAnglePositionDegrees().getDegrees(), targetState.position ); double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( @@ -48,7 +48,7 @@ private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ } private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getElevatorMotorPositionDegrees().getDegrees(), + getElevatorPositionDegrees().getDegrees(), targetState.position ); double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); @@ -59,7 +59,7 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAngleMotorPositionDegrees().getDegrees(), getAngleMotorVelocityPerSecond()) + new TrapezoidProfile.State(getAnglePositionDegrees().getDegrees(), getAngleVelocityPerSecond()) ); lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } @@ -68,7 +68,7 @@ private void generateElevatorMotorProfile(double targetPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, new TrapezoidProfile.State(targetPosition, 0), - new TrapezoidProfile.State(getElevatorMotorPositionDegrees().getDegrees(), getElevatorMotorVelocityRevolutionsPerSeconds()) + new TrapezoidProfile.State(getElevatorPositionDegrees().getDegrees(), getElevatorVelocityRevolutionsPerSeconds()) ); lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); } @@ -99,21 +99,21 @@ private double getElevatorMotorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime; } - private Rotation2d getAngleMotorPositionDegrees() { + private Rotation2d getAnglePositionDegrees() { double position = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); return new Rotation2d(position); } - private Rotation2d getElevatorMotorPositionDegrees() { + private Rotation2d getElevatorPositionDegrees() { return new Rotation2d(elevatorEncoder.getSelectedSensorPosition()); } - private double getAngleMotorVelocityPerSecond() { + private double getAngleVelocityPerSecond() { double position = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); return Conversions.revolutionsToDegrees(position); } - private double getElevatorMotorVelocityRevolutionsPerSeconds() { + private double getElevatorVelocityRevolutionsPerSeconds() { double velocity = elevatorEncoder.getSelectedSensorVelocity(); return Conversions.degreesToRevolutions(velocity); } From 6586e6d5c87df1b596d7d485178088f34589ab3b Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 19:43:02 +0200 Subject: [PATCH 32/48] Update ArmConstants.java --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index f476d7f6c..a58be56cd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -153,7 +153,5 @@ public enum ArmState { this.elevatorPosition = elevatorPosition; this.angle = angle; } - } - } From 847ed6d31b95df85c4c71991fdfd53b447eb9a30 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 20:08:56 +0200 Subject: [PATCH 33/48] fixed something --- .../frc/trigon/robot/subsystems/arm/Arm.java | 58 +++++++++---------- 1 file changed, 29 insertions(+), 29 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 f723f067c..b053268c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -19,7 +19,6 @@ public class Arm extends SubsystemBase { masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; - private final CANcoder angleEncoder = ArmConstants.ANGLE_ENCODER; private TrapezoidProfile angleMotorProfile = null, @@ -35,31 +34,11 @@ public static Arm getInstance() { private Arm() { } - private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ - double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( - getAnglePositionDegrees().getDegrees(), - targetState.position - ); - double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( - Units.degreesToRadians(targetState.position), - Units.degreesToRadians(targetState.velocity) - ); - return feedforward + pidOutput; - } - private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ - double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( - getElevatorPositionDegrees().getDegrees(), - targetState.position - ); - double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); - return feedforward + pidOutput; - } - private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePositionDegrees().getDegrees(), getAngleVelocityPerSecond()) + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityPerSecond()) ); lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } @@ -68,7 +47,7 @@ private void generateElevatorMotorProfile(double targetPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, new TrapezoidProfile.State(targetPosition, 0), - new TrapezoidProfile.State(getElevatorPositionDegrees().getDegrees(), getElevatorVelocityRevolutionsPerSeconds()) + new TrapezoidProfile.State(getElevatorPositionRevolutions().getDegrees(), getElevatorVelocityRevolutionsPerSecond()) ); lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); } @@ -99,13 +78,34 @@ private double getElevatorMotorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime; } - private Rotation2d getAnglePositionDegrees() { + private Rotation2d getAnglePosition() { double position = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); - return new Rotation2d(position); + return Rotation2d.fromRotations(position); } - private Rotation2d getElevatorPositionDegrees() { - return new Rotation2d(elevatorEncoder.getSelectedSensorPosition()); + private double getElevatorPositionRevolutions() { + return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition()); + } + + private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return feedforward + pidOutput; + } + + private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ + double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorPositionRevolutions(), + targetState.position + ); + double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); + return feedforward + pidOutput; } private double getAngleVelocityPerSecond() { @@ -113,8 +113,8 @@ private double getAngleVelocityPerSecond() { return Conversions.revolutionsToDegrees(position); } - private double getElevatorVelocityRevolutionsPerSeconds() { - double velocity = elevatorEncoder.getSelectedSensorVelocity(); + private double getElevatorVelocityRevolutionsPerSecond() { + double velocity = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); return Conversions.degreesToRevolutions(velocity); } From b19c37dc4c9ca9e3cb8a71b81cb978fe1f5e4ef3 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 20:10:08 +0200 Subject: [PATCH 34/48] fix --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b053268c2..63970db82 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -47,7 +47,7 @@ private void generateElevatorMotorProfile(double targetPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, new TrapezoidProfile.State(targetPosition, 0), - new TrapezoidProfile.State(getElevatorPositionRevolutions().getDegrees(), getElevatorVelocityRevolutionsPerSecond()) + new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond()) ); lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); } From 14a9e9780e92485f05ccb17fb261d4665bf5c550 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 20:16:14 +0200 Subject: [PATCH 35/48] !@#$%^&*()_FIX_)(*&^%$#@! --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) 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 63970db82..e3703fe74 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -7,6 +7,8 @@ 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; @@ -34,6 +36,17 @@ public static Arm getInstance() { private Arm() { } + public Command getSetTargetArmAngleCommand(Rotation2d angle){ + return new FunctionalCommand( + () -> generateAngleMotorProfile(angle), + this::setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, From 2aa4458383b14c93f3463b153a43f0a689b5aa7b Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 20:18:08 +0200 Subject: [PATCH 36/48] !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!--FIX--!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 e3703fe74..16889e4c5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -47,6 +47,17 @@ public Command getSetTargetArmAngleCommand(Rotation2d angle){ ); } + public Command getSetTargetArmElevatorCommand(Rotation2d angle){ + return new FunctionalCommand( + () -> generateElevatorMotorProfile(angle.getDegrees()), + this::setTargetElevatorFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, From a35019b8a71602854fa29a259f103433fe835856 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 21:10:58 +0200 Subject: [PATCH 37/48] FFFFFFFFFFFFFIXXXXXXXXXXXXXXX --- .../frc/trigon/robot/subsystems/arm/Arm.java | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 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 16889e4c5..0ef8f3cc8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -7,9 +7,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.SubsystemBase; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.utilities.Conversions; public class Arm extends SubsystemBase { @@ -35,6 +33,19 @@ public static Arm getInstance() { private Arm() { } + + public Command getSetToStateCommand(ArmConstants.ArmState targetState){ + if (isElevatorOpening(targetState.elevatorPosition)){ + return new SequentialCommandGroup( + getSetTargetArmAngleCommand(targetState.angle), + getSetTargetArmElevatorCommand(targetState.elevatorPosition) + ); + } + return new SequentialCommandGroup( + getSetTargetArmElevatorCommand(targetState.elevatorPosition), + getSetTargetArmAngleCommand(targetState.angle) + ); + } public Command getSetTargetArmAngleCommand(Rotation2d angle){ return new FunctionalCommand( @@ -47,9 +58,9 @@ public Command getSetTargetArmAngleCommand(Rotation2d angle){ ); } - public Command getSetTargetArmElevatorCommand(Rotation2d angle){ + public Command getSetTargetArmElevatorCommand(double position){ return new FunctionalCommand( - () -> generateElevatorMotorProfile(angle.getDegrees()), + () -> generateElevatorMotorProfile(position), this::setTargetElevatorFromProfile, (interrupted) -> { }, @@ -58,6 +69,11 @@ public Command getSetTargetArmElevatorCommand(Rotation2d angle){ ); } + + private boolean isElevatorOpening(double targetElevatorPosition){ + return targetElevatorPosition > getElevatorPositionRevolutions(); + } + private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, From 2f10641d3a85d9a6137a4842a49eba8b1199d74c Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 19 Nov 2023 21:13:24 +0200 Subject: [PATCH 38/48] Update Arm.java --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 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 0ef8f3cc8..a16863dd5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.arm; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -33,7 +32,7 @@ public static Arm getInstance() { private Arm() { } - + public Command getSetToStateCommand(ArmConstants.ArmState targetState){ if (isElevatorOpening(targetState.elevatorPosition)){ return new SequentialCommandGroup( @@ -78,15 +77,15 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { angleMotorProfile = new TrapezoidProfile( ArmConstants.ANGLE_CONSTRAINS, new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityPerSecond()) + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) ); lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } - private void generateElevatorMotorProfile(double targetPosition) { + private void generateElevatorMotorProfile(double targetElevatorPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, - new TrapezoidProfile.State(targetPosition, 0), + new TrapezoidProfile.State(targetElevatorPosition, 0), new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond()) ); lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); @@ -148,7 +147,7 @@ private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ return feedforward + pidOutput; } - private double getAngleVelocityPerSecond() { + private double getAngleVelocityDegreesPerSecond() { double position = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); return Conversions.revolutionsToDegrees(position); } From ddae22a234e299d1b565a01d95445e51cd511ac9 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 20 Nov 2023 11:03:10 +0200 Subject: [PATCH 39/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 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 a16863dd5..e5835d58e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -33,22 +33,22 @@ public static Arm getInstance() { private Arm() { } - public Command getSetToStateCommand(ArmConstants.ArmState targetState){ - if (isElevatorOpening(targetState.elevatorPosition)){ + public Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { + if (isElevatorOpening(targetState.elevatorPosition)) { return new SequentialCommandGroup( - getSetTargetArmAngleCommand(targetState.angle), - getSetTargetArmElevatorCommand(targetState.elevatorPosition) + getSetTargetAngleCommand(targetState.angle), + getSetTargetElevatorPositionCommand(targetState.elevatorPosition) ); } return new SequentialCommandGroup( - getSetTargetArmElevatorCommand(targetState.elevatorPosition), - getSetTargetArmAngleCommand(targetState.angle) + getSetTargetElevatorPositionCommand(targetState.elevatorPosition), + getSetTargetAngleCommand(targetState.angle) ); } - public Command getSetTargetArmAngleCommand(Rotation2d angle){ + public Command getSetTargetAngleCommand(Rotation2d targetAngle) { return new FunctionalCommand( - () -> generateAngleMotorProfile(angle), + () -> generateAngleMotorProfile(targetAngle), this::setTargetAngleFromProfile, (interrupted) -> { }, @@ -57,9 +57,9 @@ public Command getSetTargetArmAngleCommand(Rotation2d angle){ ); } - public Command getSetTargetArmElevatorCommand(double position){ + public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition) { return new FunctionalCommand( - () -> generateElevatorMotorProfile(position), + () -> generateElevatorMotorProfile(targetElevatorPosition), this::setTargetElevatorFromProfile, (interrupted) -> { }, @@ -69,7 +69,7 @@ public Command getSetTargetArmElevatorCommand(double position){ } - private boolean isElevatorOpening(double targetElevatorPosition){ + private boolean isElevatorOpening(double targetElevatorPosition) { return targetElevatorPosition > getElevatorPositionRevolutions(); } @@ -117,16 +117,7 @@ private double getElevatorMotorProfileTime() { return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime; } - private Rotation2d getAnglePosition() { - double position = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); - return Rotation2d.fromRotations(position); - } - - private double getElevatorPositionRevolutions() { - return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition()); - } - - private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ + private double calculateAngleMotorOutput(TrapezoidProfile.State targetState) { double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( getAnglePosition().getDegrees(), targetState.position @@ -138,7 +129,7 @@ private double calculateAngleMotorOutput(TrapezoidProfile.State targetState){ return feedforward + pidOutput; } - private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ + private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState) { double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( getElevatorPositionRevolutions(), targetState.position @@ -148,21 +139,30 @@ private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState){ } private double getAngleVelocityDegreesPerSecond() { - double position = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); - return Conversions.revolutionsToDegrees(position); + double positionRevolutions = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); + return Conversions.revolutionsToDegrees(positionRevolutions); } private double getElevatorVelocityRevolutionsPerSecond() { - double velocity = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); - return Conversions.degreesToRevolutions(velocity); + double magTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); + return Conversions.magTicksToRevolutions(magTicksPerSecond ); + } + + private Rotation2d getAnglePosition() { + double positionRevolutions = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } + + private double getElevatorPositionRevolutions() { + return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition()); } - private void stopAngleMotors(){ + private void stopAngleMotors() { masterAngleMotor.stopMotor(); followerAngleMotor.stopMotor(); } - private void stopElevatorMotors(){ + private void stopElevatorMotors() { masterElevatorMotor.stopMotor(); followerElevatorMotor.stopMotor(); } From 77bd32970ae54787e853ea6bb37419b846e41f59 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 23 Nov 2023 19:14:45 +0200 Subject: [PATCH 40/48] -=+=-__FIX__added the Deferred Command__FIX__-=+=- --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 12 ++++++++++-- .../trigon/robot/subsystems/arm/ArmConstants.java | 2 ++ 2 files changed, 12 insertions(+), 2 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 e5835d58e..96dabcf18 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.utilities.Conversions; +import java.util.Set; +import java.util.function.Supplier; + public class Arm extends SubsystemBase { private final static Arm INSTANCE = new Arm(); @@ -33,6 +36,12 @@ public static Arm getInstance() { private Arm() { } + public Command getSetCommand(ArmConstants.ArmState targetState){ + Supplier commandSupplier = () -> getSetTargetStateCommand(targetState); + Set subsystem = Set.of(this); + return new DeferredCommand(commandSupplier, subsystem); + } + public Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { if (isElevatorOpening(targetState.elevatorPosition)) { return new SequentialCommandGroup( @@ -68,7 +77,6 @@ public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition ); } - private boolean isElevatorOpening(double targetElevatorPosition) { return targetElevatorPosition > getElevatorPositionRevolutions(); } @@ -145,7 +153,7 @@ private double getAngleVelocityDegreesPerSecond() { private double getElevatorVelocityRevolutionsPerSecond() { double magTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); - return Conversions.magTicksToRevolutions(magTicksPerSecond ); + return Conversions.magTicksToRevolutions(magTicksPerSecond); } private Rotation2d getAnglePosition() { 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 a58be56cd..3d3a69c00 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -155,3 +155,5 @@ public enum ArmState { } } } + +// bcdedit /set hypervisorlaunchtype off \ No newline at end of file From 6e55d4c1a2591a8874ae2c5c267dbd6c3924cfc5 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 23 Nov 2023 19:35:32 +0200 Subject: [PATCH 41/48] updated and fixed --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 10 +++++----- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 -- 2 files changed, 5 insertions(+), 7 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 96dabcf18..613c49347 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -10,7 +10,6 @@ import frc.trigon.robot.utilities.Conversions; import java.util.Set; -import java.util.function.Supplier; public class Arm extends SubsystemBase { private final static Arm INSTANCE = new Arm(); @@ -37,12 +36,13 @@ private Arm() { } public Command getSetCommand(ArmConstants.ArmState targetState){ - Supplier commandSupplier = () -> getSetTargetStateCommand(targetState); - Set subsystem = Set.of(this); - return new DeferredCommand(commandSupplier, subsystem); + return new DeferredCommand( + () -> getCurrentSetTargetStateCommand(targetState), + Set.of(this) + ); } - public Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { + public Command getCurrentSetTargetStateCommand(ArmConstants.ArmState targetState) { if (isElevatorOpening(targetState.elevatorPosition)) { return new SequentialCommandGroup( getSetTargetAngleCommand(targetState.angle), 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 3d3a69c00..a58be56cd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -155,5 +155,3 @@ public enum ArmState { } } } - -// bcdedit /set hypervisorlaunchtype off \ No newline at end of file From 7a8efe6337dc1bf3750429410153978140553b5d Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Thu, 23 Nov 2023 19:36:58 +0200 Subject: [PATCH 42/48] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 613c49347..8674d7e5e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,7 +35,7 @@ public static Arm getInstance() { private Arm() { } - public Command getSetCommand(ArmConstants.ArmState targetState){ + public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState){ return new DeferredCommand( () -> getCurrentSetTargetStateCommand(targetState), Set.of(this) From fc4b2b569d40d641d18fed1e4ddb1e63684824ce Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 26 Nov 2023 23:57:16 +0200 Subject: [PATCH 43/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) 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 8674d7e5e..7f9e7817c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,6 +35,21 @@ public static Arm getInstance() { private Arm() { } + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double anglePercentage, double elevatorPercentage){ + generateElevatorMotorProfile(elevatorPosition, elevatorPercentage); + generateAngleMotorProfile(angle, anglePercentage); + return new FunctionalCommand( + + ); + } + + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle){ + return new SequentialCommandGroup( + getSetTargetAngleCommand(angle), + getSetTargetElevatorPositionCommand(elevatorPosition) + ); + } + public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState){ return new DeferredCommand( () -> getCurrentSetTargetStateCommand(targetState), @@ -90,6 +105,15 @@ private void generateAngleMotorProfile(Rotation2d targetAngle) { lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } + private void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) { + angleMotorProfile = new TrapezoidProfile( + Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINS, speedPercentage), + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) + ); + lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); + } + private void generateElevatorMotorProfile(double targetElevatorPosition) { elevatorMotorProfile = new TrapezoidProfile( ArmConstants.ELEVATOR_CONSTRAINS, @@ -99,6 +123,15 @@ private void generateElevatorMotorProfile(double targetElevatorPosition) { lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); } + private void generateElevatorMotorProfile(double targetElevator, double speedPercentage) { + elevatorMotorProfile = new TrapezoidProfile( + Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINS, speedPercentage), + new TrapezoidProfile.State(targetElevator, 0), + new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond()) + ); + lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); + } + private void setTargetAngleFromProfile() { if (angleMotorProfile == null) { stopAngleMotors(); From 71b1adb514ed19c13a5548de375b1421da0e155f Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 27 Nov 2023 00:11:27 +0200 Subject: [PATCH 44/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 49 ++++++++++++++++--- 1 file changed, 41 insertions(+), 8 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 7f9e7817c..4b7f82de8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,11 +35,38 @@ public static Arm getInstance() { private Arm() { } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double anglePercentage, double elevatorPercentage){ - generateElevatorMotorProfile(elevatorPosition, elevatorPercentage); - generateAngleMotorProfile(angle, anglePercentage); - return new FunctionalCommand( + public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState){ + return new DeferredCommand( + () -> getCurrentSetTargetStateCommand(targetState), + Set.of(this) + ); + } + + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double anglePercentage){ + return new DeferredCommand( + () -> getCurrentSetTargetStateCommand(elevatorPosition, angle), + Set.of(this) + ); + } + public Command getSetTargetElevatorCommand(double elevatorPosition, double elevatorPercentage){ + return new FunctionalCommand( + () -> generateElevatorMotorProfile(elevatorPosition,elevatorPercentage), + this:: setTargetElevatorFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); + } + public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage){ + return new FunctionalCommand( + () -> generateAngleMotorProfile(angle,anglePercentage), + this:: setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + this ); } @@ -50,10 +77,16 @@ public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2 ); } - public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState){ - return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(targetState), - Set.of(this) + public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle) { + if (isElevatorOpening(elevatorPosition)) { + return new SequentialCommandGroup( + getSetTargetAngleCommand(angle), + getSetTargetElevatorPositionCommand(elevatorPosition) + ); + } + return new SequentialCommandGroup( + getSetTargetElevatorPositionCommand(elevatorPosition), + getSetTargetAngleCommand(angle) ); } From a5dc6d7c2a52f4f00be655efbe07914fcbdee1da Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Mon, 4 Dec 2023 11:47:10 +0200 Subject: [PATCH 45/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 90 ++++--------------- 1 file changed, 19 insertions(+), 71 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 4b7f82de8..788fe2056 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,34 +35,31 @@ public static Arm getInstance() { private Arm() { } - public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState){ + public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState) { return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(targetState), + () -> getCurrentSetTargetStateCommand(targetState.elevatorPosition, targetState.angle, 100, 100), Set.of(this) ); } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double anglePercentage){ + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(elevatorPosition, angle), + () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), Set.of(this) ); } - public Command getSetTargetElevatorCommand(double elevatorPosition, double elevatorPercentage){ - return new FunctionalCommand( - () -> generateElevatorMotorProfile(elevatorPosition,elevatorPercentage), - this:: setTargetElevatorFromProfile, - (interrupted) -> { - }, - () -> false, - this + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { + return new SequentialCommandGroup( + getSetTargetAngleCommand(angle, 100), + getSetTargetElevatorPositionCommand(elevatorPosition, 100) ); } - public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage){ + + public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) { return new FunctionalCommand( - () -> generateAngleMotorProfile(angle,anglePercentage), - this:: setTargetAngleFromProfile, + () -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage), + this::setTargetElevatorFromProfile, (interrupted) -> { }, () -> false, @@ -70,42 +67,22 @@ public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage ); } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle){ - return new SequentialCommandGroup( - getSetTargetAngleCommand(angle), - getSetTargetElevatorPositionCommand(elevatorPosition) - ); - } - - public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle) { + public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { if (isElevatorOpening(elevatorPosition)) { return new SequentialCommandGroup( - getSetTargetAngleCommand(angle), - getSetTargetElevatorPositionCommand(elevatorPosition) - ); - } - return new SequentialCommandGroup( - getSetTargetElevatorPositionCommand(elevatorPosition), - getSetTargetAngleCommand(angle) - ); - } - - public Command getCurrentSetTargetStateCommand(ArmConstants.ArmState targetState) { - if (isElevatorOpening(targetState.elevatorPosition)) { - return new SequentialCommandGroup( - getSetTargetAngleCommand(targetState.angle), - getSetTargetElevatorPositionCommand(targetState.elevatorPosition) + getSetTargetAngleCommand(angle, angleSpeedPercentage), + getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage) ); } return new SequentialCommandGroup( - getSetTargetElevatorPositionCommand(targetState.elevatorPosition), - getSetTargetAngleCommand(targetState.angle) + getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage), + getSetTargetAngleCommand(angle, angleSpeedPercentage) ); } - public Command getSetTargetAngleCommand(Rotation2d targetAngle) { + public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage) { return new FunctionalCommand( - () -> generateAngleMotorProfile(targetAngle), + () -> generateAngleMotorProfile(angle, anglePercentage), this::setTargetAngleFromProfile, (interrupted) -> { }, @@ -114,30 +91,10 @@ public Command getSetTargetAngleCommand(Rotation2d targetAngle) { ); } - public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition) { - return new FunctionalCommand( - () -> generateElevatorMotorProfile(targetElevatorPosition), - this::setTargetElevatorFromProfile, - (interrupted) -> { - }, - () -> false, - this - ); - } - private boolean isElevatorOpening(double targetElevatorPosition) { return targetElevatorPosition > getElevatorPositionRevolutions(); } - private void generateAngleMotorProfile(Rotation2d targetAngle) { - angleMotorProfile = new TrapezoidProfile( - ArmConstants.ANGLE_CONSTRAINS, - new TrapezoidProfile.State(targetAngle.getDegrees(), 0), - new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) - ); - lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); - } - private void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) { angleMotorProfile = new TrapezoidProfile( Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINS, speedPercentage), @@ -147,15 +104,6 @@ private void generateAngleMotorProfile(Rotation2d targetAngle, double speedPerce lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); } - private void generateElevatorMotorProfile(double targetElevatorPosition) { - elevatorMotorProfile = new TrapezoidProfile( - ArmConstants.ELEVATOR_CONSTRAINS, - new TrapezoidProfile.State(targetElevatorPosition, 0), - new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond()) - ); - lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); - } - private void generateElevatorMotorProfile(double targetElevator, double speedPercentage) { elevatorMotorProfile = new TrapezoidProfile( Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINS, speedPercentage), From aecf2a145fd19af573e48e1c2735c3253313888c Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Sun, 17 Dec 2023 09:29:44 +0200 Subject: [PATCH 46/48] Update Arm.java --- .../frc/trigon/robot/subsystems/arm/Arm.java | 45 +++++++++++-------- 1 file changed, 26 insertions(+), 19 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 788fe2056..f506e96cf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -37,37 +37,33 @@ private Arm() { public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState) { return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(targetState.elevatorPosition, targetState.angle, 100, 100), + () -> getSetTargetArmPositionCommand(targetState.elevatorPosition, targetState.angle, 100, 100), Set.of(this) ); } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), + () -> getSetTargetArmPositionCommand(targetState.elevatorPosition, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage), Set.of(this) ); } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { - return new SequentialCommandGroup( - getSetTargetAngleCommand(angle, 100), - getSetTargetElevatorPositionCommand(elevatorPosition, 100) + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return new DeferredCommand( + () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), + Set.of(this) ); } - public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) { - return new FunctionalCommand( - () -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage), - this::setTargetElevatorFromProfile, - (interrupted) -> { - }, - () -> false, - this - ); + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { + return new DeferredCommand( + () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, 100, 100), + Set.of(this) + ); } - public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + private Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { if (isElevatorOpening(elevatorPosition)) { return new SequentialCommandGroup( getSetTargetAngleCommand(angle, angleSpeedPercentage), @@ -80,9 +76,20 @@ public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation ); } - public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage) { + private Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) { + return new FunctionalCommand( + () -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage), + this::setTargetElevatorFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); + } + + private Command getSetTargetAngleCommand(Rotation2d angle, double angleSpeedPercentage) { return new FunctionalCommand( - () -> generateAngleMotorProfile(angle, anglePercentage), + () -> generateAngleMotorProfile(angle, angleSpeedPercentage), this::setTargetAngleFromProfile, (interrupted) -> { }, From af693b123738e12feaa77d767e9d1cf5ac06e91f Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Wed, 20 Dec 2023 10:13:11 +0200 Subject: [PATCH 47/48] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 f506e96cf..3e5097dee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,21 +35,21 @@ public static Arm getInstance() { private Arm() { } - public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState) { + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { return new DeferredCommand( - () -> getSetTargetArmPositionCommand(targetState.elevatorPosition, targetState.angle, 100, 100), + () -> getSetTargetArmStateCommand(targetState.elevatorPosition, targetState.angle, 100, 100), Set.of(this) ); } - public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { return new DeferredCommand( - () -> getSetTargetArmPositionCommand(targetState.elevatorPosition, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage), + () -> getSetTargetArmStateCommand(targetState.elevatorPosition, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage), Set.of(this) ); } - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + public Command getSetTargetArmStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { return new DeferredCommand( () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), Set.of(this) From 6f5dbc024c5d938d5b46bb7d61b2ebafe30e799d Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky Date: Fri, 29 Dec 2023 13:03:56 +0200 Subject: [PATCH 48/48] Moved all the commands ArmCommands class. --- .../frc/trigon/robot/subsystems/arm/Arm.java | 34 ++----------------- .../robot/subsystems/arm/ArmCommands.java | 33 ++++++++++++++++++ .../robot/subsystems/arm/ArmConstants.java | 17 +++++----- 3 files changed, 45 insertions(+), 39 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 3e5097dee..7705bf168 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -35,35 +35,7 @@ public static Arm getInstance() { private Arm() { } - public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { - return new DeferredCommand( - () -> getSetTargetArmStateCommand(targetState.elevatorPosition, targetState.angle, 100, 100), - Set.of(this) - ); - } - - public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { - return new DeferredCommand( - () -> getSetTargetArmStateCommand(targetState.elevatorPosition, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage), - Set.of(this) - ); - } - - public Command getSetTargetArmStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { - return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), - Set.of(this) - ); - } - - public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { - return new DeferredCommand( - () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, 100, 100), - Set.of(this) - ); - } - - private Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { if (isElevatorOpening(elevatorPosition)) { return new SequentialCommandGroup( getSetTargetAngleCommand(angle, angleSpeedPercentage), @@ -168,7 +140,7 @@ private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState) } private double getAngleVelocityDegreesPerSecond() { - double positionRevolutions = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); + double positionRevolutions = ArmConstants.ANGLE_VELOCITY_SIGNAL.refresh().getValue(); return Conversions.revolutionsToDegrees(positionRevolutions); } @@ -178,7 +150,7 @@ private double getElevatorVelocityRevolutionsPerSecond() { } private Rotation2d getAnglePosition() { - double positionRevolutions = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); + double positionRevolutions = ArmConstants.ANGLE_POSITION_SIGNAL.refresh().getValue(); return Rotation2d.fromRotations(positionRevolutions); } 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..2ac792264 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -0,0 +1,33 @@ +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.DeferredCommand; + +import java.util.Set; + +public class ArmCommands { + private static final Arm ARM = Arm.getInstance(); + + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { + return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, 100, 100); + } + + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage); + } + + public Command getSetTargetArmStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return new DeferredCommand( + () -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), + Set.of(ARM) + ); + } + + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { + return new DeferredCommand( + () -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, 100, 100), + Set.of(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 a58be56cd..9307d9394 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.trigon.robot.utilities.Conversions; public class ArmConstants { private static final double VOLTAGE_COMPENSATION_SATURATION = 12; @@ -83,10 +84,9 @@ public class ArmConstants { ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION), ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION); - static final StatusSignal - ANGLE_MOTOR_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), - ANGLE_MOTOR_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + ANGLE_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); static { configureAngleMotors(); @@ -112,7 +112,7 @@ private static void configureElevatorMotors() { private static void configureElevatorEncoder() { ELEVATOR_ENCODER.configFactoryDefault(); ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); - ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET); + ELEVATOR_ENCODER.setSelectedSensorPosition(Conversions.offsetRead(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET, ELEVATOR_ENCODER_OFFSET)); ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); } @@ -136,8 +136,9 @@ private static void configureAngleEncoder() { configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE; configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder); - ANGLE_MOTOR_POSITION_SIGNAL.setUpdateFrequency(111); - ANGLE_MOTOR_VELOCITY_SIGNAL.setUpdateFrequency(111); + + ANGLE_POSITION_SIGNAL.setUpdateFrequency(111); + ANGLE_VELOCITY_SIGNAL.setUpdateFrequency(111); ANGLE_ENCODER.optimizeBusUtilization(); } @@ -147,10 +148,10 @@ public enum ArmState { THIRD_STATE(Rotation2d.fromDegrees(100), 3); final Rotation2d angle; - final double elevatorPosition; + final double elevatorPositionMeters; ArmState(Rotation2d angle, double elevatorPosition) { - this.elevatorPosition = elevatorPosition; + this.elevatorPositionMeters = elevatorPosition; this.angle = angle; } }