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

Commit

Permalink
done stuff ?
Browse files Browse the repository at this point in the history
  • Loading branch information
@3LucasZ
3LucasZ committed Oct 4, 2023
1 parent a7a203e commit 8c558b1
Show file tree
Hide file tree
Showing 9 changed files with 126 additions and 159 deletions.
43 changes: 24 additions & 19 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,11 @@
import frc.robot.arm.commands.SetArmVoltage;
import frc.robot.auto.AutoConstants;
import frc.robot.auto.AutoPaths;
import frc.robot.auto.pathgeneration.commands.*;
import frc.robot.auto.pathgeneration.commands.AutoIntakeAtDoubleSubstation.SubstationLocation;
import frc.robot.auto.pathgeneration.commands.AutoScore.*;
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 @@ -324,11 +322,10 @@ public void configureElevator() {

private void configureArm() {
// operator.start().onTrue(new ZeroArm(armSubsystem).withTimeout(4));
tester.povUp().whileTrue(new SetElevatorVolts(elevatorSubsystem, 3));

tester.povDown().whileTrue(new SetElevatorVolts(elevatorSubsystem, -3));
tester.povUp().whileTrue(new SetElevatorVolts(elevatorSubsystem, 6));
tester.povDown().whileTrue(new SetElevatorVolts(elevatorSubsystem, -6));
tester.povLeft().whileTrue(new SetArmVoltage(armSubsystem, 3));
tester.povRight().whileTrue(new SetArmVoltage(armSubsystem, -2));
tester.povRight().whileTrue(new SetArmVoltage(armSubsystem, -3));
// tester.leftBumper().onTrue(new SetElevatorExtension(elevatorSubsystem, 0.5));
// tester.leftTrigger().onTrue(new SetElevatorExtension(elevatorSubsystem, 1));
// tester.rightBumper().onTrue(new SetArmAngle(armSubsystem, Rotation2d.fromDegrees(0)));
Expand All @@ -337,16 +334,25 @@ private void configureArm() {
tester.leftTrigger().onTrue(new OuttakeCone(intakeSubsystem));
tester.rightBumper().onTrue(new IntakeCube(intakeSubsystem));
tester.rightTrigger().onTrue(new OuttakeCube(intakeSubsystem));
tester.a().onTrue(new AutoAlign(swerveSubsystem, 5));
tester
.y()
.onTrue(
new SequentialCommandGroup(
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_CUBE_MID),
new OuttakeCube(intakeSubsystem)));

// tester.a().onTrue(new AutoAlign(swerveSubsystem, 5));
// tester
// .y()
// .onTrue(
// new SequentialCommandGroup(
// new SetEndEffectorState(
// elevatorSubsystem,
// armSubsystem,
// SetEndEffectorState.EndEffectorPreset.SCORE_CONE_MID),
// new OuttakeCube(intakeSubsystem)));
tester.x().onTrue(new StowEndEffector(elevatorSubsystem, armSubsystem));
// tester
// .b()
// .onTrue(
// new SetEndEffectorState(
// elevatorSubsystem,
// armSubsystem,
// SetEndEffectorState.EndEffectorPreset.CUBE_GROUND_INTAKE));

// if (kIntakeEnabled && FeatureFlags.kOperatorManualArmControlEnabled) {
// operator.povUp().whileTrue(new SetArmVoltage(armSubsystem,
Expand Down Expand Up @@ -374,11 +380,10 @@ public Command getAutonomousCommand() {
Command autoPath = autoPaths.getSelectedPath();
if (kElevatorEnabled && kArmEnabled) {
return Commands.sequence(
new StowEndEffector(elevatorSubsystem, armSubsystem, this::isCurrentPieceCone),
new StowEndEffector(elevatorSubsystem, armSubsystem),
autoPath,
Commands.parallel(
new StowEndEffector(elevatorSubsystem, armSubsystem, this::isCurrentPieceCone)
.asProxy(),
new StowEndEffector(elevatorSubsystem, armSubsystem).asProxy(),
new LockSwerveX(swerveSubsystem)
.andThen(new SetAllColor(ledSubsystem, kLockSwerve))
.until(() -> isMovingJoystick(driver))));
Expand Down
32 changes: 2 additions & 30 deletions src/main/java/frc/robot/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,15 +40,13 @@
import frc.robot.drivers.CANDeviceTester;
import frc.robot.drivers.CANTestable;
import frc.robot.drivers.TalonFXFactory;
import frc.robot.elevator.ElevatorConstants;
import frc.robot.logging.DoubleSendable;
import frc.robot.logging.Loggable;
import frc.robot.swerve.helpers.Conversions;

public class Arm extends SubsystemBase implements CANTestable, Loggable {
public enum ArmPreset {
STOW_CUBE(kStowRotationCube),
STOW_CONE(kStowRotationCone),
STOW(kStowRotation),
ANY_PIECE_LOW_BACK(kAnyPieceLowBackRotation),
ANY_PIECE_LOW_FRONT(kAnyPieceLowFrontRotation),
CUBE_MID(kCubeMidRotation),
Expand Down Expand Up @@ -111,31 +109,10 @@ public void setInputVoltage(double voltage) {
armMotor.setVoltage(MathUtil.clamp(voltage, -12, 12));
}

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

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(getArmAngleElevatorRelative()),
kArmAngleMinConstraint.getDegrees(),
kArmAngleMaxConstraint.getDegrees()));
} else {
return Rotation2d.fromDegrees(
MathUtil.clamp(
Units.radiansToDegrees(getArmAngleElevatorRelative()),
kMinSafeRotation,
kMaxSafeRotation));
}
}

public double getArmAngleGroundRelative() {
if (RobotBase.isReal()) {
return Conversions.falconToRadians(armMotor.getSelectedSensorPosition(), kArmGearing);
Expand Down Expand Up @@ -227,12 +204,7 @@ public static void loadArmPreferences() {
Preferences.initDouble(ArmConstants.ArmPreferencesKeys.kDKey, ArmConstants.kArmD);

// Arm Encoder Offset
Preferences.initDouble(
ArmPreferencesKeys.kAbsoluteEncoderOffsetKey, kAbsoluteEncoderOffsetRadians);
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.STOW_CONE), kStowRotationCone.getRadians());
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.STOW_CUBE), kStowRotationCube.getRadians());
Preferences.initDouble(kArmPositionKeys.get(ArmPreset.STOW), kStowRotation.getRadians());
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.ANY_PIECE_LOW_BACK), kAnyPieceLowBackRotation.getRadians());
Preferences.initDouble(
Expand Down
44 changes: 16 additions & 28 deletions src/main/java/frc/robot/arm/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,6 @@ public final class ArmConstants {

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

public static final double kArmEncoderGearRatio = 2 / 1;
public static final double kArmRadiansPerAbsoluteEncoderRotation =
-(2 / kArmEncoderGearRatio) * Math.PI;
public static final double kAbsoluteEncoderOffsetRadians = 3.6506685;

public static final double kArmGearing = 80;
public static final double kArmLength = 0.569075;
public static final double kArmInertia = 0.410;
Expand All @@ -46,34 +41,28 @@ public final class ArmConstants {
public static final Rotation2d kArmToleranceAngle = Rotation2d.fromDegrees(5);
public static final Rotation2d kArmToleranceAngularVelocity = Rotation2d.fromDegrees(5);

public static final Rotation2d kArmAngleMinConstraint = Rotation2d.fromDegrees(0);
public static final Rotation2d kArmAngleMaxConstraint = Rotation2d.fromDegrees(180);
public static final Rotation2d kArmAngleMinConstraint = Rotation2d.fromDegrees(-37);
public static final Rotation2d kArmAngleMaxConstraint = Rotation2d.fromDegrees(160);

public static final Rotation2d kStowRotationCube = Rotation2d.fromDegrees(35);
public static final Rotation2d kStowRotationCone = Rotation2d.fromDegrees(35);
public static final Rotation2d kStowRotation = Rotation2d.fromDegrees(120);

public static final Rotation2d kDoubleSubstationRotationCube = new Rotation2d(0);
public static final Rotation2d kDoubleSubstationRotationCone = new Rotation2d(0);
public static final Rotation2d kDoubleSubstationRotationCube = new Rotation2d(157.7);
public static final Rotation2d kDoubleSubstationRotationCone = new Rotation2d(157.7);
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);
public static final Rotation2d kAnyPieceLowBackRotation = Rotation2d.fromDegrees(135);
public static final Rotation2d kAnyPieceLowFrontRotation = Rotation2d.fromDegrees(0);

public static final Rotation2d kStandingConeGroundIntakeRotation =
Rotation2d.fromDegrees(142.644);
public static final Rotation2d kTippedConeGroundIntakeRotation = Rotation2d.fromDegrees(180);
public static final Rotation2d kCubeGroundIntakeRotation = Rotation2d.fromDegrees(180);
public static final double kManualArmVoltage = 2.5;
public static final double kMinSafeRotation = 15;
public static final double kMaxSafeRotation = 130;
public static final Rotation2d kConeMidRotation = new Rotation2d(157.7);
public static final Rotation2d kCubeHighRotation = new Rotation2d(157.7);
public static final Rotation2d kConeHighRotation = new Rotation2d(157.7);
public static final Rotation2d kAnyPieceLowBackRotation = Rotation2d.fromDegrees(0);
public static final Rotation2d kAnyPieceLowFrontRotation = Rotation2d.fromDegrees(157.7);

public static final Rotation2d kStandingConeGroundIntakeRotation = Rotation2d.fromDegrees(7.5);
public static final Rotation2d kTippedConeGroundIntakeRotation = Rotation2d.fromDegrees(0);
public static final Rotation2d kCubeGroundIntakeRotation = Rotation2d.fromDegrees(-37);

public static class ArmPreferencesKeys {
public static final Map<Arm.ArmPreset, String> kArmPositionKeys =
Map.ofEntries(
Map.entry(Arm.ArmPreset.STOW_CONE, "kStowRotationCone"),
Map.entry(Arm.ArmPreset.STOW_CUBE, "kStowRotationCube"),
Map.entry(Arm.ArmPreset.STOW, "kStowRotation"),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_BACK, "kAnyPieceLowBackRotation"),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_FRONT, "kAnyPieceLowFrontRotation"),
Map.entry(Arm.ArmPreset.CUBE_MID, "kCubeMidRotation"),
Expand All @@ -89,8 +78,7 @@ public static class ArmPreferencesKeys {

public static final Map<Arm.ArmPreset, Rotation2d> kArmPositionDefaults =
Map.ofEntries(
Map.entry(Arm.ArmPreset.STOW_CONE, kStowRotationCone),
Map.entry(Arm.ArmPreset.STOW_CUBE, kStowRotationCube),
Map.entry(Arm.ArmPreset.STOW, kStowRotation),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_BACK, kAnyPieceLowBackRotation),
Map.entry(Arm.ArmPreset.CUBE_MID, kCubeMidRotation),
Map.entry(Arm.ArmPreset.CONE_MID, kConeMidRotation),
Expand Down
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::getArmAngleElevatorRelative,
armSubsystem.getArmAngleElevatorRelative(),
armSubsystem::getArmAngleGroundRelative,
armSubsystem.getArmAngleGroundRelative(),
(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.getArmAngleElevatorRelative();
armPosition = armSubsystem.getArmAngleGroundRelative();
getController().setGoal(armPosition);

if (Constants.kDebugEnabled) {
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/arm/commands/SetArmAngle.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,11 +78,11 @@ public void initialize() {
this.getName()
+ " started (preset: "
+ armPreset
+ ", setpoint rotation elevator relative: "
+ ", setpoint rotation ground relative: "
+ Units.radiansToDegrees(angleRotation)
+ " deg)"
+ ", current arm rotation elevator relative: "
+ Units.radiansToDegrees(armSubsystem.getArmAngleElevatorRelative())
+ ", current arm rotation ground relative: "
+ Units.radiansToDegrees(armSubsystem.getArmAngleGroundRelative())
+ " deg)");
}
}
Expand All @@ -105,8 +105,8 @@ public void end(boolean interrupted) {
this.getName()
+ " ended (preset: "
+ armPreset
+ ", current arm rotation elevator relative: "
+ Units.radiansToDegrees(armSubsystem.getArmAngleElevatorRelative())
+ ", current arm rotation ground relative: "
+ Units.radiansToDegrees(armSubsystem.getArmAngleGroundRelative())
+ " deg, "
+ ", rotation: "
+ Units.radiansToDegrees(angleRotation)
Expand Down
15 changes: 5 additions & 10 deletions src/main/java/frc/robot/elevator/Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,7 @@

public class Elevator extends SubsystemBase implements CANTestable, Loggable {
public enum ElevatorPreset {
STOW_CONE(kConeStowPosition),
STOW_CUBE(kCubeStowPosition),
STOW(kStowPosition),
CUBE_HIGH(kCubeHighPosition),
CONE_HIGH(kConeHighPosition),
ANY_PIECE_MID(kAnyPieceMidPosition),
Expand Down Expand Up @@ -94,7 +93,9 @@ private void configureRealHardware() {
elevatorFollowerMotor.setInverted(InvertType.OpposeMaster);
elevatorFollowerMotor.setNeutralMode(NeutralMode.Brake);
}

// slam down == -1 == lower current threshold
// middle movement == 0 == very high current threshold
// slam up == 1 == medium current threshold
public boolean isMotorCurrentSpiking(int dir) {
if (RobotBase.isReal()) {
if (dir == -1) {
Expand Down Expand Up @@ -142,10 +143,6 @@ public void off() {
System.out.println("Elevator off");
}

public boolean isSafeFromArm() {
return getElevatorPosition() > kSafeForArmMinPosition;
}

@Override
public void periodic() {
SmartDashboard.putNumber(
Expand Down Expand Up @@ -180,9 +177,7 @@ public static void loadElevatorPreferences() {
Preferences.initDouble(ElevatorPreferencesKeys.kIKey, kElevatorI);
Preferences.initDouble(ElevatorPreferencesKeys.kDKey, kElevatorD);
// Elevator Preset Preferences
Preferences.initDouble(kElevatorPositionKeys.get(ElevatorPreset.STOW_CONE), kConeStowPosition);
Preferences.initDouble(
kElevatorPositionKeys.get(Elevator.ElevatorPreset.STOW_CUBE), kCubeStowPosition);
Preferences.initDouble(kElevatorPositionKeys.get(ElevatorPreset.STOW), kStowPosition);
Preferences.initDouble(
kElevatorPositionKeys.get(Elevator.ElevatorPreset.CUBE_HIGH), kCubeHighPosition);
Preferences.initDouble(
Expand Down
Loading

0 comments on commit 8c558b1

Please sign in to comment.