Skip to content

Commit

Permalink
Arm pre bringup
Browse files Browse the repository at this point in the history
  • Loading branch information
@manthan-acharya
manthan-acharya committed Feb 4, 2024
1 parent 62465e6 commit 05e06b4
Show file tree
Hide file tree
Showing 6 changed files with 111 additions and 101 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,8 +206,8 @@ private void configureButtonBindings() {
// () -> -controller.getLeftX(),
// () -> -controller.getRightX()));
// controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive));
controller.a().onTrue(arm.home());
arm.setDefaultCommand(arm.runToSetpoint());

// arm.setDefaultCommand(arm.runToSetpoint());
// controller
// .a()
// .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(),
Expand Down
82 changes: 47 additions & 35 deletions src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,19 +2,20 @@

import static frc.robot.subsystems.superstructure.SuperstructureConstants.ArmConstants.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
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.subsystems.drive.DriveConstants;
import frc.robot.util.EqualsUtil;
import frc.robot.util.LoggedTunableNumber;
import java.util.function.BooleanSupplier;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

public class Arm extends SubsystemBase {
Expand Down Expand Up @@ -50,6 +51,12 @@ public class Arm extends SubsystemBase {
private final MechanismRoot2d armRoot;
private final MechanismLigament2d armMeasured;

private final Timer homingTimer = new Timer();

private boolean homed = false;

private double setpoint = 0.0;

// private final MechanismLigament2d armSetpoint;

public Arm(ArmIO io) {
Expand All @@ -59,12 +66,16 @@ public Arm(ArmIO io) {

// Create a mechanism
armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite));
armRoot = armMechanism.getRoot("Arm Joint", armOrigin2d.getX(), armOrigin2d.getY());
armRoot =
armMechanism.getRoot(
"Arm Joint",
armOrigin2d.getX() + DriveConstants.driveConfig.trackwidthX() / 2.0,
armOrigin2d.getY());
armMeasured =
new MechanismLigament2d(
"Arm Measured",
armLength,
inputs.armAnglePosition.getDegrees(),
Units.radiansToDegrees(inputs.armAnglePositionRads),
2.0,
new Color8Bit(Color.kBlack));
armRoot.append(armMeasured);
Expand All @@ -88,55 +99,51 @@ public void periodic() {
LoggedTunableNumber.ifChanged(
hashCode(), () -> armIO.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD);
LoggedTunableNumber.ifChanged(
hashCode(), () -> armIO.setFF(kS.get(), kV.get(), kA.get(), kG.get()));
hashCode(), () -> armIO.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG);
LoggedTunableNumber.ifChanged(
hashCode(),
constraints -> armIO.setProfileConstraints(constraints[0], constraints[1]),
armVelocity,
armAcceleration);

setpoint = Units.degreesToRadians(armDesiredSetpoint.get());
// Home if not already homed
if (!homed && DriverStation.isEnabled()) {
armIO.setVoltage(-1.0);
if (EqualsUtil.epsilonEquals(
inputs.armVelocityRadsPerSec, 0.0, Units.degreesToRadians(1.0))) {
homingTimer.start();
} else {
homingTimer.reset();
}

if (homingTimer.hasElapsed(0.5)) {
armIO.setPosition(0);
homed = true;
}
} else {
setSetpoint(setpoint);
}

if (DriverStation.isDisabled()) {
armIO.stop();
}

if (coastSupplier.getAsBoolean()) armIO.setBrakeMode(false);

// Logs
armMeasured.setAngle(inputs.armAnglePosition);
armMeasured.setAngle(Units.radiansToDegrees(inputs.armAnglePositionRads));
// armSetpoint.setAngle(inputs.armReferencePosition);
Logger.recordOutput("Arm/SetpointAngle", inputs.armReferencePosition);
Logger.recordOutput("Arm/MeasuredAngle", inputs.armAnglePosition);
Logger.recordOutput("Arm/VelocityRadsPerSec", inputs.armVelocityRadsPerSec);
Logger.recordOutput("Arm/Mechanism", armMechanism);
}

public Command runToSetpoint() {
return run(
() -> {
if (inputs.homed) {
Rotation2d setpoint =
Rotation2d.fromDegrees(
MathUtil.clamp(
armDesiredSetpoint.get(), minAngle.getDegrees(), maxAngle.getDegrees()));
setSetpoint(setpoint);
}
});
public void setVoltage(double volts) {
armIO.setVoltage(volts);
}

public Command home() {
return run(() -> armIO.setVoltage(-1.0))
.until(
() -> {
boolean homed = EqualsUtil.epsilonEquals(inputs.armVelocityRadsPerSec, 0.0, 0.01);
System.out.println(homed);
return homed;
})
.andThen(() -> armIO.setHome());
}

public void setSetpoint(Rotation2d setpoint) {
if (disableSupplier.getAsBoolean() || !inputs.homed) return;
armIO.setSetpoint(setpoint.getRadians());
public void setSetpoint(double setpointRads) {
if (disableSupplier.getAsBoolean() || !homed) return;
armIO.setSetpoint(setpointRads);
}

public void setBrakeMode(boolean enabled) {
Expand All @@ -151,4 +158,9 @@ public void setOverrides(BooleanSupplier disableSupplier, BooleanSupplier coastS
public void stop() {
armIO.stop();
}

@AutoLogOutput(key = "Arm/Homed")
public boolean homed() {
return homed;
}
}
28 changes: 10 additions & 18 deletions src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIO.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,15 @@
package frc.robot.subsystems.superstructure.Arm;

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

public interface ArmIO {

@AutoLog
class ArmIOInputs {
public Rotation2d armAnglePosition = new Rotation2d();
public Rotation2d armReferencePosition = new Rotation2d();
public boolean hasFoc = false;
public boolean hasAbsoluteSensor = false;
public double armAnglePositionRads = 0.0;
public double armTrajectorySetpointRads = 0.0;
public double armVelocityRadsPerSec = 0.0;
public boolean homed = false;
public boolean atSetpoint = false;
public double[] armAppliedVolts = new double[] {};
public double[] armCurrentAmps = new double[] {};
public double[] armTorqueCurrentAmps = new double[] {};
Expand All @@ -26,6 +24,9 @@ default void setSetpoint(double setpointRads) {}
/** Run motors at voltage */
default void setVoltage(double volts) {}

/** Run motors at current */
default void setCurrent(double amps) {}

/** Set brake mode enabled */
default void setBrakeMode(boolean enabled) {}

Expand All @@ -39,17 +40,8 @@ default void setPID(double p, double i, double d) {}
default void setProfileConstraints(
double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {}

/**
* Stops motors
*
* @params none!
*/
default void stop() {}
default void setPosition(double positionRads) {}

/**
* Set current position to home
*
* @params none!
*/
default void setHome() {}
/** Stops motors */
default void stop() {}
}
30 changes: 14 additions & 16 deletions src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,15 +14,13 @@
import com.ctre.phoenix6.signals.GravityTypeValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;

public class ArmIOKrakenFOC implements ArmIO {
private final TalonFX leaderMotor;
private final TalonFX followerMotor;
Double home = null;
private final StatusSignal<Double> armPositionRotations;
private final StatusSignal<Double> armReferencePositionRotations;
private final StatusSignal<Double> armTrajectorySetpointPositionRotations;
private final StatusSignal<Double> armVelocityRPS;
private final StatusSignal<Double>[] armAppliedVoltage = new StatusSignal[2];
private final StatusSignal<Double>[] armOutputCurrent = new StatusSignal[2];
Expand All @@ -44,7 +42,7 @@ public ArmIOKrakenFOC() {
leaderConfig.Voltage.PeakReverseVoltage = 12.0;
leaderConfig.MotorOutput.Inverted =
leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
leaderConfig.Feedback.RotorToSensorRatio = 1.0 / reduction;
leaderConfig.Feedback.SensorToMechanismRatio = reduction;
leaderConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;

controllerConfig =
Expand All @@ -63,7 +61,7 @@ public ArmIOKrakenFOC() {
new MotionMagicConfigs()
.withMotionMagicCruiseVelocity(
Units.radiansToRotations(profileConstraints.cruiseVelocityRadPerSec()))
.withMotionMagicCruiseVelocity(
.withMotionMagicAcceleration(
Units.radiansToRotations(profileConstraints.accelerationRadPerSec2()));
leaderMotor.getConfigurator().apply(leaderConfig, 1);

Expand All @@ -72,7 +70,7 @@ public ArmIOKrakenFOC() {
followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
// Status signals
armPositionRotations = leaderMotor.getPosition();
armReferencePositionRotations = leaderMotor.getClosedLoopReference();
armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference();
armVelocityRPS = leaderMotor.getVelocity();
armAppliedVoltage[0] = leaderMotor.getMotorVoltage();
armAppliedVoltage[1] = followerMotor.getMotorVoltage();
Expand All @@ -84,9 +82,9 @@ public ArmIOKrakenFOC() {
armTempCelsius[1] = followerMotor.getDeviceTemp();

BaseStatusSignal.setUpdateFrequencyForAll(
50,
100,
armPositionRotations,
armReferencePositionRotations,
armTrajectorySetpointPositionRotations,
armVelocityRPS,
armAppliedVoltage[0],
armAppliedVoltage[1],
Expand All @@ -99,9 +97,11 @@ public ArmIOKrakenFOC() {
}

public void updateInputs(ArmIOInputs inputs) {
inputs.hasFoc = true;

BaseStatusSignal.refreshAll(
armPositionRotations,
armReferencePositionRotations,
armTrajectorySetpointPositionRotations,
armVelocityRPS,
armAppliedVoltage[0],
armAppliedVoltage[1],
Expand All @@ -112,9 +112,9 @@ public void updateInputs(ArmIOInputs inputs) {
armTempCelsius[0],
armTempCelsius[1]);

inputs.armAnglePosition = Rotation2d.fromRotations(armPositionRotations.getValue() - home);
inputs.armReferencePosition =
Rotation2d.fromRotations(armReferencePositionRotations.getValue() - home);
inputs.armAnglePositionRads = Units.rotationsToRadians(armPositionRotations.getValue());
inputs.armTrajectorySetpointRads =
Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue());
inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue());
inputs.armAppliedVolts =
new double[] {armAppliedVoltage[0].getValue(), armAppliedVoltage[1].getValue()};
Expand All @@ -124,7 +124,6 @@ public void updateInputs(ArmIOInputs inputs) {
new double[] {armTorqueCurrent[0].getValue(), armTorqueCurrent[1].getValue()};
inputs.armTempCelcius =
new double[] {armTempCelsius[0].getValue(), armTempCelsius[1].getValue()};
inputs.homed = home != null;
}

@Override
Expand Down Expand Up @@ -172,8 +171,7 @@ public void setProfileConstraints(
}

@Override
public void setHome() {
home = armPositionRotations.getValue();
leaderMotor.setPosition(0.0, 0.01);
public void setPosition(double positionRads) {
leaderMotor.setPosition(Units.radiansToRotations(positionRads));
}
}
Loading

0 comments on commit 05e06b4

Please sign in to comment.