Skip to content

Arm #3

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

Open
NolanKnight wants to merge 4 commits into
base: main
Choose a base branch
from
+449 −6
Open

Arm #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
9f7ceca
added arm subsystem classes
NolanKnight Jan 9, 2026
a671358
arm in subsystem manager
NolanKnight Jan 9, 2026
932bdf3
arm controls and simulation
NolanKnight Jan 9, 2026
821bcc6
robot model
NolanKnight Jan 9, 2026
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
34 changes: 34 additions & 0 deletions assets/Robot_Potato/config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
{
"name": "Potato",
"rotations": [
{
"axis": "x",
"degrees": 90
}
],
"position": [
0,
0,
0
],
"cameras": [],
"components": [
{
"zeroedRotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "z",
"degrees": -90
}
],
"zeroedPositions": [
0,
0,
0
]
}
]
}
Binary file added assets/Robot_Potato/model.glb
View file
Open in desktop
Binary file not shown.
Binary file added assets/Robot_Potato/model_0.glb
View file
Open in desktop
Binary file not shown.
28 changes: 23 additions & 5 deletions src/main/java/com/team973/frc2025/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
package com.team973.frc2025;

import com.team973.frc2025.subsystems.DriveController;
import com.team973.frc2025.subsystems.arm.ArmIO;
import com.team973.lib.devices.GreyPigeonIO;
import com.team973.lib.util.AllianceCache;
import com.team973.lib.util.Joystick;
Expand All @@ -21,29 +22,37 @@ public class Robot extends TimedRobot {

private final GreyPigeonIO m_pigeon = m_subsystemManager.getPigeon();
private final DriveController m_driveController = m_subsystemManager.getDriveController();
private final ArmIO m_arm = m_subsystemManager.getArm();

private final Joystick m_driverStick =
new Joystick(0, Joystick.Type.XboxController, m_logger.subLogger("driverStick"));
private final Joystick m_coDriverStick =
new Joystick(1, Joystick.Type.XboxController, m_logger.subLogger("coDriverStick"));

private void syncSensors() {
m_driveController.syncSensors();
m_arm.syncSensors();
}

private void updateSubsystems() {
m_driveController.update();
m_arm.update();
}

private void resetSubsystems() {
m_driveController.reset();
m_arm.reset();
}

private void log() {
m_subsystemManager.log();
m_driveController.log();
m_arm.log();
}

private void updateJoysticks() {
m_driverStick.update();
m_coDriverStick.update();
}

public Robot() {
Expand All @@ -53,6 +62,7 @@ public Robot() {

@Override
public void robotPeriodic() {
syncSensors();
log();
updateJoysticks();
}
Expand All @@ -71,8 +81,6 @@ public void teleopInit() {

@Override
public void teleopPeriodic() {
syncSensors();

double allianceScalar = 1.0;
if (AllianceCache.Get().get() == Alliance.Red) {
// Our gyroscope is blue-centric meaning that facing away from the alliance wall
Expand All @@ -95,16 +103,26 @@ public void teleopPeriodic() {
allianceScalar * m_driverStick.getLeftXAxis() * 0.7,
m_driverStick.getRightXAxis() * 0.8);

if (m_coDriverStick.getAButtonPressed()) {
m_arm.setTargetPreset(ArmIO.Preset.One);
} else if (m_coDriverStick.getBButtonPressed()) {
m_arm.setTargetPreset(ArmIO.Preset.Two);
} else if (m_coDriverStick.getXButtonPressed()) {
m_arm.setTargetPreset(ArmIO.Preset.Three);
} else if (m_coDriverStick.getYButtonPressed()) {
m_arm.setState(ArmIO.State.Manual);
}

m_arm.setManualInput(m_coDriverStick.getLeftYAxis());

updateSubsystems();
}

@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {
syncSensors();
}
public void disabledPeriodic() {}

@Override
public void testInit() {}
Expand Down
34 changes: 34 additions & 0 deletions src/main/java/com/team973/frc2025/shared/RobotInfo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,40 @@ public class RobotInfo {

public static final DriveInfo DRIVE_INFO = new DriveInfo();

public static final ArmInfo ARM_INFO = new ArmInfo();

public static class ArmInfo {
public final int MOTOR_ID = 20;

public final double MOTOR_GEAR_RATIO = (10.0 / 50.0) * (20.0 / 68.0) * (17.0 / 84.0);
public final double LENGTH_METERS = 0.5461;

public final double ARM_KS = 0.0;
public final double ARM_KV = 0.0;
public final double ARM_KA = 0.0;
public final double ARM_KP = 2.0;
public final double ARM_KI = 0.0;
public final double ARM_KD = 0.0;

public final double ARM_MOTION_MAGIC_CRUISE_VELOCITY = 10.0;
public final double ARM_MOTION_MAGIC_ACCELERATION = 10.0;
public final double ARM_MOTION_MAGIC_JERK = 0.0;

public final double ARM_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.0;

public final double STATOR_CURRENT_LIMIT = 60.0;
public final double SUPPLY_CURRENT_LIMIT = 40.0;

public final double PEAK_FORWARD_VOLTAGE = 12.0;
public final double PEAK_REVERSE_VOLTAGE = -12.0;

public final double MANUAL_INPUT_TO_VOLTS = 12.0;

public final double PRESET_ONE = 0.0;
public final double PRESET_TWO = 60.0;
public final double PRESET_THREE = -60.0;
}

public static class DriveInfo {
public final int STATUS_SIGNAL_FREQUENCY = 200;

Expand Down
123 changes: 123 additions & 0 deletions src/main/java/com/team973/frc2025/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
package com.team973.frc2025.subsystems.arm;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.team973.frc2025.shared.RobotInfo;
import com.team973.lib.devices.GreyTalonFX;
import com.team973.lib.util.Logger;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;

public class Arm extends ArmIO {
protected static final RobotInfo.ArmInfo m_armInfo = RobotInfo.ARM_INFO;

protected final Logger m_logger;

protected final GreyTalonFX m_motor;

private double m_targetPostionDeg;
private double m_manualInput;

public Arm(Logger logger) {
m_logger = logger;
m_motor =
new GreyTalonFX(
m_armInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight"));

TalonFXConfiguration motorConfig = new TalonFXConfiguration();

motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;

motorConfig.Slot0.kS = m_armInfo.ARM_KS;
motorConfig.Slot0.kV = m_armInfo.ARM_KV;
motorConfig.Slot0.kA = m_armInfo.ARM_KA;
motorConfig.Slot0.kP = m_armInfo.ARM_KP;
motorConfig.Slot0.kI = m_armInfo.ARM_KI;
motorConfig.Slot0.kD = m_armInfo.ARM_KD;

motorConfig.MotionMagic.MotionMagicCruiseVelocity = m_armInfo.ARM_MOTION_MAGIC_CRUISE_VELOCITY;
motorConfig.MotionMagic.MotionMagicAcceleration = m_armInfo.ARM_MOTION_MAGIC_ACCELERATION;
motorConfig.MotionMagic.MotionMagicJerk = m_armInfo.ARM_MOTION_MAGIC_JERK;

motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod =
m_armInfo.ARM_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD;

motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;

motorConfig.CurrentLimits.StatorCurrentLimit = m_armInfo.STATOR_CURRENT_LIMIT;
motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
motorConfig.CurrentLimits.SupplyCurrentLimit = m_armInfo.SUPPLY_CURRENT_LIMIT;
motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;

motorConfig.Voltage.PeakForwardVoltage = m_armInfo.PEAK_FORWARD_VOLTAGE;
motorConfig.Voltage.PeakReverseVoltage = m_armInfo.PEAK_REVERSE_VOLTAGE;

m_motor.setConfig(motorConfig);
m_motor.setPosition(0.0);

m_targetPostionDeg = Preset.One.getPositionDeg();
m_manualInput = 0.0;
}

@Override
public Pose3d getPose() {
return new Pose3d(
new Translation3d(),
new Rotation3d(
0, 0, Math.toRadians(motorRotationsToDeg(m_motor.getPosition().getValueAsDouble()))));
}

@Override
public GreyTalonFX getMotor() {
return m_motor;
}

private double degToMotorRotations(double deg) {
return (deg / 360.0) / m_armInfo.MOTOR_GEAR_RATIO;
}

private double motorRotationsToDeg(double motorPostion) {
return (motorPostion * m_armInfo.MOTOR_GEAR_RATIO) * 360.0;
}

@Override
public void setTargetPreset(Preset preset) {
m_targetPostionDeg = preset.getPositionDeg();
setState(State.ClosedLoop);
}

@Override
public void setManualInput(double input) {
m_manualInput = input;
}

@Override
public double getTargetPositionMotorRot() {
return degToMotorRotations(m_targetPostionDeg);
}

@Override
public double getManualInput() {
return m_manualInput;
}

@Override
public void syncSensors() {}

@Override
public void log() {
m_motor.log();

m_logger.log(
"currentPostionDeg", motorRotationsToDeg(m_motor.getPosition().getValueAsDouble()));
m_logger.log("targetPostionDeg", m_targetPostionDeg);
m_logger.log("manualInput", m_manualInput);

m_logger.log("state", getState().toString());
}

@Override
public void reset() {}
}
63 changes: 63 additions & 0 deletions src/main/java/com/team973/frc2025/subsystems/arm/ArmIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
package com.team973.frc2025.subsystems.arm;

import com.team973.frc2025.shared.RobotInfo;
import com.team973.lib.devices.GreyTalonFX;
import com.team973.lib.util.StateMap;
import com.team973.lib.util.Subsystem;
import edu.wpi.first.math.geometry.Pose3d;

public abstract class ArmIO extends Subsystem<ArmIO.State> {
private final StateMap<State> m_stateMap;

protected static final RobotInfo.ArmInfo m_armInfo = RobotInfo.ARM_INFO;

public enum State {
ClosedLoop,
Manual,
Off
}

public enum Preset {
One(m_armInfo.PRESET_ONE),
Two(m_armInfo.PRESET_TWO),
Three(m_armInfo.PRESET_THREE);

private final double m_positionDeg;

private Preset(double positionDeg) {
m_positionDeg = positionDeg;
}

public double getPositionDeg() {
return m_positionDeg;
}
}

@SuppressWarnings("unchecked")
public ArmIO() {
super(State.Off);

m_stateMap =
new StateMap<>(
State.class,
new StateMap.Entry<>(State.ClosedLoop, new ArmStates.ClosedLoop(this)),
new StateMap.Entry<>(State.Manual, new ArmStates.Manual(this)),
new StateMap.Entry<>(State.Off, new ArmStates.Off(this)));
}

public StateMap<State> getStateMap() {
return m_stateMap;
}

public abstract Pose3d getPose();

public abstract GreyTalonFX getMotor();

public abstract void setTargetPreset(Preset preset);

public abstract void setManualInput(double volts);

public abstract double getTargetPositionMotorRot();

public abstract double getManualInput();
}
Loading