Skip to content
This repository has been archived by the owner on Apr 11, 2024. It is now read-only.

Commit

Permalink
Arm and Elevator are moving to set position and set angle now :)
Browse files Browse the repository at this point in the history
  • Loading branch information
@3LucasZ
3LucasZ committed Oct 1, 2023
1 parent 28d3631 commit d036e74
Show file tree
Hide file tree
Showing 9 changed files with 83 additions and 113 deletions.
22 changes: 18 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import static frc.robot.swerve.SwerveConstants.*;

import com.pathplanner.lib.server.PathPlannerServer;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
Expand All @@ -22,6 +23,7 @@
import frc.robot.Constants.FeatureFlags;
import frc.robot.arm.Arm;
import frc.robot.arm.ArmConstants;
import frc.robot.arm.commands.SetArmAngle;
import frc.robot.arm.commands.SetArmVoltage;
import frc.robot.arm.commands.ZeroArm;
import frc.robot.auto.AutoConstants;
Expand All @@ -32,6 +34,7 @@
import frc.robot.drivers.CANTestable;
import frc.robot.elevator.Elevator;
import frc.robot.elevator.commands.SetElevatorVolts;
import frc.robot.elevator.commands.SetEndEffectorState;
import frc.robot.elevator.commands.StowEndEffector;
import frc.robot.intake.Intake;
import frc.robot.intake.commands.*;
Expand Down Expand Up @@ -317,8 +320,8 @@ private void configureIntake() {
}

public void configureElevator() {
operator.povUp().onTrue(new SetElevatorVolts(elevatorSubsystem, 5));
operator.povDown().onTrue(new SetElevatorVolts(elevatorSubsystem, -5));
operator.povUp().onTrue(new SetElevatorVolts(elevatorSubsystem, 2));
operator.povDown().onTrue(new SetElevatorVolts(elevatorSubsystem, -2));
if (kArmEnabled) {
operator
.leftTrigger()
Expand All @@ -328,8 +331,19 @@ public void configureElevator() {

private void configureArm() {
operator.start().onTrue(new ZeroArm(armSubsystem).withTimeout(4));
operator.povLeft().onTrue(new SetArmVoltage(armSubsystem, 5));
operator.povRight().onTrue(new SetArmVoltage(armSubsystem, -5));
operator.povLeft().onTrue(new SetArmVoltage(armSubsystem, 2));
operator.povRight().onTrue(new SetArmVoltage(armSubsystem, -2));
operator.x().onTrue(new SetArmAngle(armSubsystem, Rotation2d.fromDegrees(0)));
operator.b().onTrue(new SetArmAngle(armSubsystem, Rotation2d.fromDegrees(90)));
operator
.rightTrigger()
.onTrue(
new SequentialCommandGroup(
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_CUBE_MID),
new OuttakeCube(intakeSubsystem)));

if (kIntakeEnabled && FeatureFlags.kOperatorManualArmControlEnabled) {
operator.povUp().whileTrue(new SetArmVoltage(armSubsystem, ArmConstants.kManualArmVoltage));
Expand Down
114 changes: 43 additions & 71 deletions src/main/java/frc/robot/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
Expand Down Expand Up @@ -71,7 +70,6 @@ public enum ArmPreset {

private WPI_TalonFX armMotor;
private final ArmFeedforward armFeedforward = new ArmFeedforward(kArmS, kArmG, kArmV, kArmA);
private final DutyCycleEncoder armEncoder = new DutyCycleEncoder(kArmEncoderDIOPort);

public Arm() {
if (RobotBase.isReal()) {
Expand All @@ -81,32 +79,27 @@ public Arm() {
}

System.out.println("Arm initialized");
zeroArmEncoderElevatorRelative();
off();

getLayout(kDriverTabName).add(new ZeroArm(this));
setNeutral();
}

private void configureRealHardware() {
armMotor = TalonFXFactory.createDefaultTalon(kArmCANDevice);
armMotor.setInverted(TalonFXInvertType.CounterClockwise);
armEncoder.setDistancePerRotation(kArmRadiansPerAbsoluteEncoderRotation);
armEncoder.reset();
armMotor.setInverted(TalonFXInvertType.Clockwise);

if (FeatureFlags.kCalibrationMode) {
armMotor.setNeutralMode(NeutralMode.Coast);
} else {
armMotor.setNeutralMode(NeutralMode.Brake);
}

if (FeatureFlags.kUseAbsoluteEncoderToInitializeRelative) {
double absoluteEncoderRadians =
getAbsoluteEncoderDistanceRad() + kAbsoluteEncoderOffsetRadians;
armMotor.setSelectedSensorPosition(
Conversions.radiansToFalcon(absoluteEncoderRadians, kArmGearing));
} else {
armMotor.setSelectedSensorPosition(0);
}
// if (FeatureFlags.kUseAbsoluteEncoderToInitializeRelative) {
// double absoluteEncoderRadians =
// getAbsoluteEncoderDistanceRad() + kAbsoluteEncoderOffsetRadians;
// armMotor.setSelectedSensorPosition(
// Conversions.radiansToFalcon(absoluteEncoderRadians, kArmGearing));
// } else {
// armMotor.setSelectedSensorPosition(0);
// }
}

public double calculateFeedForward(double angleRadians, double velocity) {
Expand All @@ -118,68 +111,46 @@ public void setInputVoltage(double voltage) {
}

public boolean isSafeFromElevator() {
return (Units.radiansToDegrees(getArmPositionElevatorRelative()) <= kMaxSafeRotation
&& Units.radiansToDegrees(getArmPositionElevatorRelative()) >= kMinSafeRotation);
}

public void zeroArmEncoderElevatorRelative() {
armMotor.setSelectedSensorPosition(
Conversions.radiansToFalcon(kElevatorAngleOffset, kArmGearing));
return (Units.radiansToDegrees(getArmAngleElevatorRelative()) <= kMaxSafeRotation
&& Units.radiansToDegrees(getArmAngleElevatorRelative()) >= kMinSafeRotation);
}

public double getAbsoluteEncoderDistanceRad() {
if (FeatureFlags.kUseAbsoluteEncoderToInitializeRelative) {
return armEncoder.getAbsolutePosition() * kArmRadiansPerAbsoluteEncoderRotation;
} else {
return armEncoder.getDistance();
}
public void zeroArmEncoderGroundRelative() {
armMotor.setSelectedSensorPosition(Conversions.radiansToFalcon(kArmAngleInit, kArmGearing));
}

public Rotation2d getClosestSafePosition(double elevatorPosition) {
if (elevatorPosition > ElevatorConstants.kSafeForArmMinPosition) {
return Rotation2d.fromDegrees(
MathUtil.clamp(
Units.radiansToDegrees(getArmPositionElevatorRelative()),
Units.radiansToDegrees(getArmAngleElevatorRelative()),
kArmAngleMinConstraint.getDegrees(),
kArmAngleMaxConstraint.getDegrees()));
} else {
return Rotation2d.fromDegrees(
MathUtil.clamp(
Units.radiansToDegrees(getArmPositionElevatorRelative()),
Units.radiansToDegrees(getArmAngleElevatorRelative()),
kMinSafeRotation,
kMaxSafeRotation));
}
}

public double getArmPositionGroundRelative() {
public double getArmAngleGroundRelative() {
if (RobotBase.isReal()) {
if (FeatureFlags.kUseRelativeArmEncoder) {
return Conversions.falconToRadians(armMotor.getSelectedSensorPosition(), kArmGearing);
} else {
double absoluteEncoderDistance;
if (kUsePrefs) {
absoluteEncoderDistance =
getAbsoluteEncoderDistanceRad()
+ Preferences.getDouble(
ArmPreferencesKeys.kAbsoluteEncoderOffsetKey, kAbsoluteEncoderOffsetRadians);
} else {
absoluteEncoderDistance = getAbsoluteEncoderDistanceRad() + kAbsoluteEncoderOffsetRadians;
}
return absoluteEncoderDistance;
}
return Conversions.falconToRadians(armMotor.getSelectedSensorPosition(), kArmGearing);
} else return armSim.getAngleRads();
}

public double getArmPositionElevatorRelative() {
return getArmPositionGroundRelative() - kElevatorAngleOffset;
public double getArmAngleElevatorRelative() {
return getArmAngleGroundRelative() - kElevatorAngleOffset;
}

public void zeroThroughboreEncoder() {
armEncoder.reset();
public boolean isMotorCurrentSpiking() {
return armMotor.getSupplyCurrent() > kArmCurrentThreshold;
}

public boolean isMotorCurrentSpiking() {
return armMotor.getSupplyCurrent() > kZeroArmCurrentThreshold;
public void setNeutral() {
armMotor.neutralOutput();
}

public void off() {
Expand All @@ -195,18 +166,19 @@ public void periodic() {
// armEncoder.getAbsolutePosition());
// SmartDashboard.putNumber(
// "Arm Raw Absolute Encoder distance", getAbsoluteEncoderDistanceRad());
SmartDashboard.putNumber("Arm angle ground relative rad", getArmPositionGroundRelative());
SmartDashboard.putNumber("Arm angle elevator relative rad", getArmPositionElevatorRelative());
SmartDashboard.putNumber("Arm Current Draw", armMotor.getSupplyCurrent());
SmartDashboard.putNumber(
"Arm motor open loop voltage", armMotor.getMotorOutputPercent() * 12);
SmartDashboard.putBoolean("Arm encoder connected", armEncoder.isConnected());
SmartDashboard.putNumber(
"Arm angle position ground relative",
Units.radiansToDegrees(getArmPositionGroundRelative()));
SmartDashboard.putNumber(
"Arm angle position elevator relative",
Units.radiansToDegrees(getArmPositionElevatorRelative()));
// SmartDashboard.putNumber("Arm angle ground relative rad",
// getArmPositionGroundRelative());
// SmartDashboard.putNumber("Arm angle elevator relative rad",
// getArmPositionElevatorRelative());
// SmartDashboard.putNumber("Arm Current Draw", armMotor.getSupplyCurrent());
// SmartDashboard.putNumber(
// "Arm motor open loop voltage", armMotor.getMotorOutputPercent() * 12);
// SmartDashboard.putNumber(
// "Arm angle position ground relative",
// Units.radiansToDegrees(getArmPositionGroundRelative()));
// SmartDashboard.putNumber(
// "Arm angle position elevator relative",
// Units.radiansToDegrees(getArmPositionElevatorRelative()));
}
}

Expand All @@ -222,10 +194,10 @@ public boolean CANTest() {
@Override
public void logInit() {
getLayout(kDriverTabName).add(this);
getLayout(kDriverTabName).add(new ZeroArm(this));
getLayout(kDriverTabName)
.add(
"Angle",
new DoubleSendable(() -> Math.toDegrees(getArmPositionGroundRelative()), "Gyro"));
"Angle", new DoubleSendable(() -> Math.toDegrees(getArmAngleGroundRelative()), "Gyro"));
getLayout(kDriverTabName).add(armMotor);
getLayout(kDriverTabName).add(new SetArmAngle(this, ArmPreset.STANDING_CONE_GROUND_INTAKE));
}
Expand Down Expand Up @@ -287,6 +259,7 @@ public static void loadArmPreferences() {
kDoubleSubstationRotationCube.getRadians());
}

// --SIM--
private final SingleJointedArmSim armSim =
new SingleJointedArmSim(
DCMotor.getFalcon500(kNumArmMotors),
Expand Down Expand Up @@ -314,7 +287,7 @@ private void configureSimHardware() {
new MechanismLigament2d(
"Arm",
kArmLength,
Units.radiansToDegrees(getArmPositionElevatorRelative()) - 90,
Units.radiansToDegrees(getArmAngleElevatorRelative()) - 90,
10,
new Color8Bit(Color.kBlue));
}
Expand All @@ -330,11 +303,10 @@ public void simulationPeriodic() {

private void simulationOutputToDashboard() {
SmartDashboard.putNumber(
"Arm angle position ground relative",
Units.radiansToDegrees(getArmPositionGroundRelative()));
"Arm angle position ground relative", Units.radiansToDegrees(getArmAngleGroundRelative()));
SmartDashboard.putNumber(
"Arm angle position elevator relative",
Units.radiansToDegrees(getArmPositionElevatorRelative()));
Units.radiansToDegrees(getArmAngleElevatorRelative()));
SmartDashboard.putNumber("Current Draw", armSim.getCurrentDrawAmps());
SmartDashboard.putNumber("Arm Sim Voltage", armMotor.getMotorOutputPercent() * 12);
}
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/arm/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import frc.robot.drivers.CanDeviceId;
import java.util.Map;

Expand All @@ -17,7 +18,8 @@ public final class ArmConstants {
public static final String kArmCanBus = "rio";
public static final CanDeviceId kArmCANDevice = new CanDeviceId(kArmMotorID, kArmCanBus);
public static final int kArmSimulationID = 16;
public static final int kArmEncoderDIOPort = 8;

public static final double kArmAngleInit = Units.degreesToRadians(-37);

public static final double kArmEncoderGearRatio = 2 / 1;
public static final double kArmRadiansPerAbsoluteEncoderRotation =
Expand Down Expand Up @@ -52,7 +54,7 @@ public final class ArmConstants {

public static final Rotation2d kDoubleSubstationRotationCube = new Rotation2d(0);
public static final Rotation2d kDoubleSubstationRotationCone = new Rotation2d(0);
public static final Rotation2d kCubeMidRotation = new Rotation2d(0);
public static final Rotation2d kCubeMidRotation = new Rotation2d(157.7);
public static final Rotation2d kConeMidRotation = new Rotation2d(0);
public static final Rotation2d kCubeHighRotation = new Rotation2d(0);
public static final Rotation2d kConeHighRotation = new Rotation2d(0);
Expand Down Expand Up @@ -107,6 +109,5 @@ public static class ArmPreferencesKeys {
}

// Zero Arm
public static final double kZeroArmVoltage = -6.0;
public static final double kZeroArmCurrentThreshold = 20.0;
public static final double kArmCurrentThreshold = 20.0;
}
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/arm/commands/KeepArmAtPosition.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@ public class KeepArmAtPosition extends ProfiledPIDCommand {
public KeepArmAtPosition(Arm armSubsystem) {
super(
new ProfiledPIDController(kArmP, kArmI, kArmD, kArmProfileContraints),
armSubsystem::getArmPositionElevatorRelative,
armSubsystem.getArmPositionElevatorRelative(),
armSubsystem::getArmAngleElevatorRelative,
armSubsystem.getArmAngleElevatorRelative(),
(output, setpoint) ->
armSubsystem.setInputVoltage(
output + armSubsystem.calculateFeedForward(setpoint.position, setpoint.velocity)));
Expand All @@ -40,7 +40,7 @@ public KeepArmAtPosition(Arm armSubsystem) {
@Override
public void initialize() {
super.initialize();
armPosition = armSubsystem.getArmPositionElevatorRelative();
armPosition = armSubsystem.getArmAngleElevatorRelative();
getController().setGoal(armPosition);

if (Constants.kDebugEnabled) {
Expand Down
17 changes: 8 additions & 9 deletions src/main/java/frc/robot/arm/commands/SetArmAngle.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
package frc.robot.arm.commands;

import static frc.robot.arm.ArmConstants.*;
import static frc.robot.elevator.ElevatorConstants.kElevatorAngleOffset;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
Expand All @@ -35,11 +34,11 @@ public class SetArmAngle extends ProfiledPIDCommand {
public SetArmAngle(Arm armSubsystem, Rotation2d angleRotation2d) {
super(
new ProfiledPIDController(kArmP, kArmI, kArmD, kArmProfileContraints),
armSubsystem::getArmPositionGroundRelative,
armSubsystem::getArmAngleGroundRelative,
MathUtil.clamp(
angleRotation2d.getRadians() + kElevatorAngleOffset,
kArmAngleMinConstraint.getRadians() + kElevatorAngleOffset,
kArmAngleMaxConstraint.getRadians() + kElevatorAngleOffset),
angleRotation2d.getRadians(),
kArmAngleMinConstraint.getRadians(),
kArmAngleMaxConstraint.getRadians()),
(output, setpoint) ->
armSubsystem.setInputVoltage(
output + armSubsystem.calculateFeedForward(setpoint.position, setpoint.velocity)),
Expand Down Expand Up @@ -71,7 +70,7 @@ public void initialize() {
// update at runtime in case robot prefs changed
if (armPreset != null) {
angleRotation = armSubsystem.getArmSetpoint(armPreset).getRadians();
getController().setGoal(angleRotation + kElevatorAngleOffset);
getController().setGoal(angleRotation);
}

if (Constants.kDebugEnabled) {
Expand All @@ -83,7 +82,7 @@ public void initialize() {
+ Units.radiansToDegrees(angleRotation)
+ " deg)"
+ ", current arm rotation elevator relative: "
+ Units.radiansToDegrees(armSubsystem.getArmPositionElevatorRelative())
+ Units.radiansToDegrees(armSubsystem.getArmAngleElevatorRelative())
+ " deg)");
}
}
Expand All @@ -94,7 +93,7 @@ public void execute() {
SmartDashboard.putNumber(
"Arm setpoint", Units.radiansToDegrees(getController().getSetpoint().position));
SmartDashboard.putNumber(
"Arm position", Units.radiansToDegrees(armSubsystem.getArmPositionGroundRelative()));
"Arm position", Units.radiansToDegrees(armSubsystem.getArmAngleGroundRelative()));
}

@Override
Expand All @@ -107,7 +106,7 @@ public void end(boolean interrupted) {
+ " ended (preset: "
+ armPreset
+ ", current arm rotation elevator relative: "
+ Units.radiansToDegrees(armSubsystem.getArmPositionElevatorRelative())
+ Units.radiansToDegrees(armSubsystem.getArmAngleElevatorRelative())
+ " deg, "
+ ", rotation: "
+ Units.radiansToDegrees(angleRotation)
Expand Down
Loading

0 comments on commit d036e74

Please sign in to comment.