Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/arm 2024 #7

Open
wants to merge 22 commits into
base: main
Choose a base branch
from
Open

Feature/arm 2024 #7

Show file tree
Hide file tree
Changes from 20 commits
Commits
Show all changes
22 commits
Select commit Hold shift + click to select a range
29607bf
Starting out
GaiaZano05 Dec 8, 2023
3391e5f
Changing name
GaiaZano05 Dec 8, 2023
3b19694
Change Name
GaiaZano05 Dec 13, 2023
893d53b
Katzuv don't be mad....
GaiaZano05 Dec 15, 2023
693a41f
Changing to armLoggedInputs
GaiaZano05 Dec 15, 2023
71c1971
Change to ArmLoggedInputs
GaiaZano05 Dec 15, 2023
edcb9a8
delete AutoLog
GaiaZano05 Dec 15, 2023
7f455bf
Add ArmLoggedInputs
GaiaZano05 Dec 15, 2023
8e12c73
Add currentLimit
GaiaZano05 Dec 15, 2023
700f911
Use ArmInputsLogged
GaiaZano05 Dec 15, 2023
3ead9e7
Fixing inputs
GaiaZano05 Dec 15, 2023
38d3357
ArmIOReal
GaiaZano05 Dec 15, 2023
a0e58c8
Changing functions to work with phoenix6
GaiaZano05 Dec 15, 2023
b51421d
Adding IO func
GaiaZano05 Dec 15, 2023
76c0dde
Change ArmLoggedInputs to ArmInputs
GaiaZano05 Dec 17, 2023
7da4eee
Adding configurations to Constants
GaiaZano05 Dec 17, 2023
30b28f2
Change name of inputs class
GaiaZano05 Dec 17, 2023
19dbdff
Add variable to ArmIO
GaiaZano05 Dec 17, 2023
4d60400
Delete some configurations and add to Constants
GaiaZano05 Dec 17, 2023
9dc2d35
Katsuv Fixes
GaiaZano05 Dec 19, 2023
f06bf8d
Starting fixes
GaiaZano05 Dec 22, 2023
0f4c619
dont touch my sphaget
GaiaZano05 Dec 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .idea/misc.xml
View file
Open in desktop

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

24 changes: 11 additions & 13 deletions src/main/java/frc/robot/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,15 @@
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Robot;
import frc.robot.subsystems.arm.commands.ArmXboxControl;
import org.littletonrobotics.junction.Logger;

public class Arm extends SubsystemBase {
private static final ArmInputsAutoLogged inputs = new ArmInputsAutoLogged();
private static Arm INSTANCE;
private final ArmInputs inputs = new ArmInputs();
private final ArmIO io;
private final Mechanism2d mechanism = new Mechanism2d(
3, 3
Expand All @@ -28,24 +26,24 @@ public class Arm extends SubsystemBase {
new MechanismLigament2d("Shoulder", ArmConstants.SHOULDER_LENGTH, 0)
);
private final MechanismLigament2d elbow = shoulder.append(
new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0, 10, new Color8Bit(Color.kPurple))
new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0)
);
private Command lastCommand = null;
private Command currentCommand = null;
private boolean changedToDefaultCommand = false;

private Arm(ArmIO io) {
this.io = io;

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

public static Arm getINSTANCE() {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

if (INSTANCE == null) {
if (Robot.isReal()) {
INSTANCE = new Arm(new ArmIOSim(inputs));

} else {
INSTANCE = new Arm(new ArmIOSim(inputs));
}
Comment on lines +35 to -48
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

INSTANCE = new Arm();
}
return INSTANCE;
}
Expand Down Expand Up @@ -94,7 +92,7 @@ public Translation2d getEndPosition() {
return ArmIO.armKinematics.forwardKinematics(shoulderAngle, shoulderAngle + elbowAngle - Math.PI);
}

public ArmIO.ArmInputs getInputs() {
public ArmInputs getInputs() {
return inputs;
}

Expand Down
33 changes: 32 additions & 1 deletion src/main/java/frc/robot/subsystems/arm/ArmConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
package frc.robot.subsystems.arm;

import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.TalonFXInvertType;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.Slot1Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;

Expand Down Expand Up @@ -34,8 +39,32 @@ public class ArmConstants {
public static final double TICKS_PER_RADIAN_SHOULDER = SHOULDER_FALCON_TICKS_PER_REVOLUTION / (Math.PI * 2);
public static final double ELBOW_FALCON_TICKS_PER_REVOLUTION = 2048 * ELBOW_GEARING;
public static final double TICKS_PER_RADIAN_ELBOW = ELBOW_FALCON_TICKS_PER_REVOLUTION / (Math.PI * 2);
public static final TalonFXInvertType MAIN_CLOCKWISE = TalonFXInvertType.Clockwise;
public static final boolean MAIN_CLOCKWISE = true;
public static final TalonFXInvertType AUX_CLOCKWISE = TalonFXInvertType.Clockwise;
public static final TalonFXConfiguration shoulderMainMotorConfig = new TalonFXConfiguration();
public static final TalonFXConfiguration shoulderAuxMotorConfig = new TalonFXConfiguration();
public static final TalonFXConfiguration elbowMainMotorConfig = new TalonFXConfiguration();
public static final TalonFXConfiguration elbowAuxMotorConfig = new TalonFXConfiguration();
public static final double SHOULDER_FEED_FORWARD_MULTIPLIER = 0;
public static final double ELBOW_FEED_FORWARD_MULTIPLIER = 0;
public static final int SHOULDER_VELOCITY = 0;
public static final int ELBOW_VELOCITY = 0;

public static final Slot0Configs SHOULDER_PID= new Slot0Configs()
.withKP(ArmConstants.shoulderP)
.withKI(ArmConstants.shoulderI)
.withKD(ArmConstants.shoulderD);
Comment on lines +53 to +56
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


public static final Slot1Configs ELBOW_PID = new Slot1Configs()
.withKP(ArmConstants.elbowP)
.withKI(ArmConstants.elbowI)
.withKD(ArmConstants.elbowD);

public static final double SENSOR_TO_MECHANISM_RATIO= 1;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

public static final double ROTOR_TO_SENSOR_RATIO= 1;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is used when defining an external sensor for the TalonFX. I think it's only CANCoders.





public static final double ELBOW_ZERO_POSITION = 360 - 53.33; //[degrees]
public static final double SHOULDER_ZERO_POSITION = 180 - 65.53; //[degrees]
Expand All @@ -45,6 +74,8 @@ public class ArmConstants {
public static final double END_POSITION_LOWER_X_LIMIT = 0; //[cm]
public static final double SHOULDER_ABSOLUTE_ENCODER_OFFSET = 0.4090 - SHOULDER_ZERO_POSITION / 360.0;
public static final double ELBOW_ABSOLUTE_ENCODER_OFFSET = 0.57396956434 - ELBOW_ZERO_POSITION / 360.0;
public static final double CURRENT_LIMIT = 10;


//PID
public static final double shoulderP = 4.0;
Expand Down
26 changes: 6 additions & 20 deletions src/main/java/frc/robot/subsystems/arm/ArmIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package frc.robot.subsystems.arm;

import edu.wpi.first.math.geometry.Translation2d;
import org.littletonrobotics.junction.AutoLog;

public interface ArmIO {
katzuv marked this conversation as resolved.
Show resolved Hide resolved
double currentShoulderAngle = 0;
double currentElbowAngle = 0;

ArmKinematics armKinematics = new ArmKinematics(ArmConstants.SHOULDER_LENGTH, ArmConstants.ELBOW_LENGTH);

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

String getSubsystemName();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this necessary?


void updateInputs(ArmInputs inputs);

enum ControlMode {
PRECENT_OUTPUT,
POSITION
}

@AutoLog
class ArmInputs {

public double shoulderAngleSetPoint = 0;
double shoulderAppliedVoltage = 0;
double elbowAppliedVoltage = 0;
double shoulderAppliedCurrent = 0;
double elbowAppliedCurrent = 0;
double[] shoulderTipPose = new double[2];
double[] endEffectorPositionSetPoint = new double[2];
double shoulderAngle = 0;
double elbowAngleSetpoint = 0;
double elbowAngleRelative = 0;
double elbowAngleAbsolute = 0;
Translation2d endEffectorPose;
ControlMode shoulderControlMode = ControlMode.PRECENT_OUTPUT;
ControlMode elbowControlMode = ControlMode.PRECENT_OUTPUT;
}
}
Loading