Feature/arm 2024 #7

GaiaZano05 · 2023-12-08T11:39:43Z

Changing the arm subsystem to work with phoenix 6.

katzuv · 2023-12-08T12:00:52Z

The ArmIO is missing.

katzuv

Make sure to add all files and use Phoenix 6 API only.

src/main/java/frc/robot/subsystems/arm/Arm.java

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

katzuv · 2023-12-16T17:51:41Z

src/main/java/frc/robot/subsystems/arm/ArmInputsLogged.java

+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class ArmInputsLogged implements LoggableInputs {


Why not use @Autolog?

src/main/java/frc/robot/subsystems/arm/ArmIO.java

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

katzuv · 2023-12-16T23:09:20Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+
+            public boolean armIsOutOfFrame () {
+                Translation2d elbowJoint = getElbowJointPosition(), endPosition = getEndPosition();
+                return !(elbowJoint.getX() < 0) || !(endPosition.getX() < 0);


Suggested change

return !(elbowJoint.getX() < 0) || !(endPosition.getX() < 0);

return elbowJoint.getX() >= 0 || endPosition.getX() >= 0;

But is that true, though? Shouldn't positive values indicate "in frame"?

katzuv · 2023-12-16T23:10:07Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+            }
+
+            public boolean armIsOutOfFrame () {
+                Translation2d elbowJoint = getElbowJointPosition(), endPosition = getEndPosition();


What benefit does creating these variables have?

katzuv · 2023-12-16T23:11:28Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+            @Override
+            public String getSubsystemName () {
+                return "Arm";
+            }


Why is that needed?

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

vichik123

Great job!

vichik123 · 2023-12-21T10:24:34Z

src/main/java/frc/robot/subsystems/arm/Arm.java

+
+    private Arm() {
+        if (Robot.isReal()) {
+            io = new ArmIOReal();
+        } else {
+            io = new ArmIOReal();
+        }
    }

    public static Arm getINSTANCE() {
        if (INSTANCE == null) {
-            if (Robot.isReal()) {
-                INSTANCE = new Arm(new ArmIOSim(inputs));
-
-            } else {
-                INSTANCE = new Arm(new ArmIOSim(inputs));
-            }


The arm object doesn't need to know whether or not it's real. In the future (already exists in the robot template), there is going to be a static initializing function for each subsystem, to which the robot mode will be passed. So for now, the creation of the io should be done in the getInstance function.

What happens if there are more than 2 implementations? For example, Falcon 500 and NEO swerve implementations (Robotinyo? 👀).

vichik123 · 2023-12-21T10:24:58Z

src/main/java/frc/robot/subsystems/arm/Arm.java

+            io = new ArmIOReal();
+        } else {
+            io = new ArmIOReal();
+        }
    }

    public static Arm getINSTANCE() {


Suggested change

public static Arm getINSTANCE() {

public static Arm getInstance() {

vichik123 · 2023-12-21T10:26:41Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+
+
+
+    ArmIOReal() {


Suggested change

ArmIOReal() {

public ArmIOReal() {

vichik123 · 2023-12-21T10:27:11Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT;
+        elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT;


Isn't this constant 10 ampere?

Did we do that during the season?

vichik123 · 2023-12-21T10:34:01Z

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

+                .withKI(ArmConstants.elbowI)
+                .withKD(ArmConstants.elbowD);
+
+    public static final double SENSOR_TO_MECHANISM_RATIO= 1;


This is used to calculate gear ratio in the motor. When using the getPosition() function it's necessary, because it returns a position times the gear ratio. However, I believe that the parallel function used in the phoenix 5 version was getRotorPosition(), with the gear ratio taken into account later. Here https://pro.docs.ctr-electronics.com/en/latest/docs/migration/migration-guide/status-signals-guide.html#using-status-signals.

vichik123 · 2023-12-21T10:47:21Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        angle = AngleUtil.normalize(angle);
+        Rotation2d error = new Rotation2d(angle).minus(new Rotation2d(currentShoulderAngle));
+        if (elbowEncoder.isConnected()) {
+            motorControlRequest = new PositionVoltage(angle);


Why are both the shoulder and the elbow updating the same member?

vichik123 · 2023-12-21T10:47:31Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+            elbowMainMotor.setControl(new PositionDutyCycle(angle, velocity, true,
+                    elbowFeedforward * ffMultiplier, 1, true)
+                    .withEnableFOC(true));


Same here as before.

vichik123 · 2023-12-21T10:51:00Z

src/main/java/frc/robot/subsystems/arm/ArmIOSim.java

I understand the appeal of the SingleJointedArmSim class. However, I recommend using the TalonFXSim class, because the "api" is the same as the TalonFX class, allowing us to find bugs in the real usage of the api before the robot comes.

vichik123 · 2023-12-21T10:53:26Z

src/main/java/frc/robot/subsystems/arm/ArmIOSim.java

-    public void updateInputs() {
+    public void updateInputs(ArmInputs inputs) {


This isn't actually necessary. You could pass the inputs through the io constructor and have it as a member in the io class (both real and sim).

vichik123 · 2023-12-21T10:54:16Z

src/main/java/frc/robot/subsystems/arm/ArmInputs.java

I would say to put this class in the io interface, but this is enormous, so I get it.

katzuv · 2023-12-22T00:54:21Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+    }
+
+
+    private void setShoulderJointAngle(double angle, double ffMultiplier, int velocity) {


Everything in your code (including IO implementations, subsystem code, arm constants, etc) should work with Rotation2d objects. That way you won't need to cast back and forth everywhere.

katzuv · 2023-12-22T20:18:12Z

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

+    public static final Slot0Configs SHOULDER_PID= new Slot0Configs()
+                .withKP(ArmConstants.shoulderP)
+                .withKI(ArmConstants.shoulderI)
+                .withKD(ArmConstants.shoulderD);


You don't need to create the gains as separate gains. Just write them directly here.

katzuv · 2023-12-22T20:19:43Z

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -29,27 +30,12 @@ default void setElbowP(double kP) {
    default void updateInputs() {
    }

+    String getSubsystemName();


Why is this necessary?

katzuv · 2023-12-22T20:56:07Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        this.shoulderEncoder = new DutyCycleEncoder(Ports.ArmPorts.SHOULDER_ENCODER);
+        this.elbowEncoder = new DutyCycleEncoder(Ports.ArmPorts.ELBOW_ENCODER);
+
+        this.shoulderMainMotor = new TalonFX(Ports.ArmPorts.SHOULDER_MAIN_MOTOR);
+
+        this.shoulderAuxMotor = new TalonFX(Ports.ArmPorts.SHOULDER_AUX_MOTOR);
+
+        this.elbowMainMotor = new TalonFX(Ports.ArmPorts.ELBOW_MAIN_MOTOR);
+
+        this.elbowAuxMotor = new TalonFX(Ports.ArmPorts.ELBOW_AUX_MOTOR);
+
+
+
+        shoulderAuxMotor.setControl(new StrictFollower(shoulderMainMotor.getDeviceID()));
+        elbowAuxMotor.setControl(new StrictFollower(elbowMainMotor.getDeviceID()));
+
+


Too much whitespace leads to tedious scrolling.

katzuv · 2023-12-22T21:11:48Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        shoulderMainMotorConfig.Voltage.PeakForwardVoltage = ArmConstants.VOLT_COMP_SATURATION;
+        shoulderMainMotorConfig.Voltage.PeakReverseVoltage = -ArmConstants.VOLT_COMP_SATURATION;
+        shoulderMainMotorConfig.Slot0 = SHOULDER_PID;
+        elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT;
+        elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT;
+        shoulderMainMotorConfig.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM_RATIO;
+        shoulderMainMotorConfig.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR_RATIO;
+        shoulderMainMotor.getConfigurator().apply(shoulderMainMotorConfig);
+
+        shoulderMainMotor.setNeutralMode(NeutralModeValue.Brake);
+        shoulderMainMotor.setInverted(MAIN_CLOCKWISE);
+
+        elbowMainMotorConfig.Voltage.PeakForwardVoltage = ArmConstants.VOLT_COMP_SATURATION;
+        elbowMainMotorConfig.Voltage.PeakReverseVoltage = -ArmConstants.VOLT_COMP_SATURATION;
+        elbowMainMotorConfig.Slot1 = ELBOW_PID;
+        elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT;
+        elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT;
+        elbowMainMotorConfig.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM_RATIO;
+        elbowMainMotorConfig.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR_RATIO;


All of that should be in the constants file. Use the with* methods and remove the constants, this will be cleaner and shorter -- you won't need one line to define the constant and another to use it, you'll just have one line total.

katzuv · 2023-12-22T21:15:31Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        angle = AngleUtil.normalize(angle);
+        Rotation2d error = new Rotation2d(angle).minus(new Rotation2d(currentShoulderAngle));
+        if (shoulderEncoder.isConnected()) {
+            motorControlRequest = new PositionVoltage(angle);


You shouldn't instantiate a new object every time, create it only once and change it using withPosition().

katzuv · 2023-12-22T21:17:28Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+        return Rotation2d.fromRotations(shoulderEncoder.getAbsolutePosition() - shoulderOffset);
+    }
+
+    public Rotation2d getElbowJointAngle() {
+        return Rotation2d.fromRotations(elbowEncoder.getAbsolutePosition() - elbowOffset);


It's better to set the offset and ration once instead of calculating it yourself. You can find the relevant methods here.

katzuv · 2023-12-22T21:18:32Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

+    public void setElbowJointAngle(Rotation2d angle) {
+        setElbowJointAngle(angle.getRadians(), ELBOW_FEED_FORWARD, ELBOW_VELOCITY);
+    }
+
+    /**
+     * Sets the angle of the elbow joint.
+     *
+     * @param angle desired angle. [degrees]
+     */
+    private void setElbowJointAngle(double angle, double ffMultiplier, int velocity) {


These two should accept the same types, hence the private method should also receive a Rotation2d.

katzuv · 2023-12-22T21:23:21Z

src/main/java/frc/robot/subsystems/arm/ArmInputs.java

+
+
+    @Override
+    public void toLog(LogTable table) {


In your case you can remove toLog and fromLog and just use @AutoLog.

katzuv · 2023-12-27T23:47:46Z

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

@@ -199,7 +193,7 @@ private void setElbowJointAngle(double angle, double ffMultiplier, int velocity)
    public Translation2d getEndPosition() {
        Rotation2d shoulderAngle = getShoulderJointAngle();
        Rotation2d elbowAngle = getElbowJointAngle();
-        return kinematics.forwardKinematics(shoulderAngle.getRadians(),shoulderAngle.getRadians() + elbowAngle.getRadians() - Math.PI);
+        return kinematics.forwardKinematics(shoulderAngle.getRadians(), shoulderAngle.getRadians() + elbowAngle.getRadians() - Math.PI);


Kinematics should work with Rotation2d too.

GaiaZano05 added 2 commits December 8, 2023 13:29

Starting out

29607bf

Changing name

3391e5f

GaiaZano05 assigned katzuv, vichik123 and GaiaZano05 Dec 8, 2023

katzuv self-requested a review December 11, 2023 00:14

katzuv requested changes Dec 11, 2023

View reviewed changes

GaiaZano05 added 12 commits December 13, 2023 11:22

Change Name

3b19694

Katzuv don't be mad....

893d53b

Changing to armLoggedInputs

693a41f

Change to ArmLoggedInputs

71c1971

delete AutoLog

edcb9a8

Add ArmLoggedInputs

7f455bf

Add currentLimit

8e12c73

Use ArmInputsLogged

700f911

Fixing inputs

3ead9e7

ArmIOReal

38d3357

Changing functions to work with phoenix6

a0e58c8

Adding IO func

b51421d

katzuv requested changes Dec 16, 2023

View reviewed changes

GaiaZano05 added 6 commits December 17, 2023 19:17

Change ArmLoggedInputs to ArmInputs

76c0dde

Adding configurations to Constants

7da4eee

Change name of inputs class

30b28f2

Add variable to ArmIO

19dbdff

Delete some configurations and add to Constants

4d60400

Katsuv Fixes

9dc2d35

vichik123 requested changes Dec 21, 2023

View reviewed changes

katzuv requested changes Dec 22, 2023

View reviewed changes

Starting fixes

f06bf8d

katzuv requested changes Dec 22, 2023

View reviewed changes

dont touch my sphaget

0f4c619

katzuv requested changes Dec 27, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/arm 2024 #7

Feature/arm 2024 #7

GaiaZano05 commented Dec 8, 2023

katzuv commented Dec 8, 2023

katzuv left a comment

katzuv Dec 16, 2023

katzuv Dec 16, 2023

katzuv Dec 16, 2023

katzuv Dec 16, 2023

vichik123 left a comment

vichik123 Dec 21, 2023

katzuv Dec 22, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

vichik123 Dec 21, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 27, 2023

	return !(elbowJoint.getX() < 0) \|\| !(endPosition.getX() < 0);
	return elbowJoint.getX() >= 0 \|\| endPosition.getX() >= 0;

	public static Arm getINSTANCE() {
	public static Arm getInstance() {

		elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT;
		elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT;

		public void updateInputs() {
		public void updateInputs(ArmInputs inputs) {

		}


		private void setShoulderJointAngle(double angle, double ffMultiplier, int velocity) {

Feature/arm 2024 #7

Are you sure you want to change the base?

Feature/arm 2024 #7

Conversation

GaiaZano05 commented Dec 8, 2023

katzuv commented Dec 8, 2023

katzuv left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vichik123 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment