Skip to content

Commit

Permalink
140 arm and wrist subsystem checkup (#175)
Browse files Browse the repository at this point in the history 
* halfway through massive changes

* basic pid for wrist + arm

Co-authored-by: Rishi Reddy <RishiReddy1002@users.noreply.github.com>

* final touchups

Co-authored-by: Rishi Reddy <RishiReddy1002@users.noreply.github.com>

* spotless

* add ARMWRIST stuff

* tuneablelele

* even more fox 111!!!!11!!!1!1

* little stuff - cleaned up the Arm & Wrist subsystem

* moved wrist subsystem

* got basically all code properly written including,
* current limit for wrist
* flipping for wrist
* PID loops for wrist and arm

Co-authored-by: Matthew Milunic <mmilunicmobile@users.noreply.github.com>
Co-authored-by: AlexNunemacher <AlexNunemacher@users.noreply.github.com>

* properly added back in upper and lower bound for wrist and arm and made sure all constants were in

* Fixed two very minor bugs

* Formatted and functional. (Added PID gains that arent 0)

---------

Co-authored-by: Asa Wang <axxwang13@gmail.com>
Co-authored-by: Rishi Reddy <RishiReddy1002@users.noreply.github.com>
Co-authored-by: Unciaur <52894100+Unciaur@users.noreply.github.com>
Co-authored-by: wibwib <ray@dokholyan.org>
Co-authored-by: Elliptication <amajha@hershey.k12.pa.us>
Co-authored-by: Liam P. <liamthomaspotter@gmail.com>
Co-authored-by: Matthew Milunic <mmilunicmobile@users.noreply.github.com>
Co-authored-by: AlexNunemacher <AlexNunemacher@users.noreply.github.com>
  • Loading branch information
@mmilunicmobile @asunflour
@RishiReddy1002 @Unciaur @urayed @Elliptication @Sloghead11 @AlexNunemacher
9 people authored Feb 7, 2025
1 parent dfdf480 commit e106f80
Show file tree
Hide file tree
Showing 16 changed files with 465 additions and 284 deletions.
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,7 @@ public void configureBindings() {
Set.of(
robotContainer.armSubsystem,
robotContainer.elevatorSubsystem,
robotContainer.wristSubsystem,
robotContainer.stateManager));
NamedCommands.registerCommand("arm", armCommand.asProxy());

Expand All @@ -264,6 +265,7 @@ public void configureBindings() {
Set.of(
robotContainer.armSubsystem,
robotContainer.elevatorSubsystem,
robotContainer.wristSubsystem,
robotContainer.stateManager));
NamedCommands.registerCommand("preparm", prepArmCommand.asProxy());

Expand Down
126 changes: 79 additions & 47 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@
import frc.robot.subsystems.arm.ArmPivotIOSim;
import frc.robot.subsystems.arm.ArmPivotIOTalonFX;
import frc.robot.subsystems.arm.ArmSubsystem;
import frc.robot.subsystems.arm.WristIONeo550;
import frc.robot.subsystems.arm.WristIOSim;
import frc.robot.subsystems.climber.ClimberIOSim;
import frc.robot.subsystems.climber.ClimberIOTalonFX;
import frc.robot.subsystems.climber.ClimberSubsystem;
Expand All @@ -55,6 +53,9 @@
import frc.robot.subsystems.vision.VisionIOLimelight;
import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
import frc.robot.subsystems.vision.VisionIOPhotonVisionSimML;
import frc.robot.subsystems.wrist.WristIONeo550;
import frc.robot.subsystems.wrist.WristIOSim;
import frc.robot.subsystems.wrist.WristSubsystem;
import java.util.Set;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
Expand All @@ -74,6 +75,7 @@ public class RobotContainer {
public ElevatorSubsystem elevatorSubsystem;
public ClimberSubsystem climberSubsystem;
public ArmSubsystem armSubsystem;
public WristSubsystem wristSubsystem;
public Vision vision;
public LightsSubsystem lights;

Expand Down Expand Up @@ -103,7 +105,8 @@ public RobotContainer() {
() -> drivetrain.getRobotPose().getRotation()));
gripperSubsystem = new GripperSubsystem(new GripperIOFalcon());
elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX());
armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX(), new WristIONeo550());
armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX());
wristSubsystem = new WristSubsystem(new WristIONeo550());
climberSubsystem = new ClimberSubsystem(new ClimberIOTalonFX());
lights = new LightsSubsystem();

Expand All @@ -127,13 +130,15 @@ public RobotContainer() {

gripperSubsystem = new GripperSubsystem(new GripperIOSim());
elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOSim());
armSubsystem = new ArmSubsystem(new ArmPivotIOSim(), new WristIOSim());
armSubsystem = new ArmSubsystem(new ArmPivotIOSim());
wristSubsystem = new WristSubsystem(new WristIOSim());
intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim());
climberSubsystem = new ClimberSubsystem(new ClimberIOSim());
lights = new LightsSubsystem();
}

stateManager = new SuperstructureStateManager(elevatorSubsystem, armSubsystem);
stateManager =
new SuperstructureStateManager(elevatorSubsystem, armSubsystem, wristSubsystem);

auto = new Auto(drivetrain, this);
configureBindings();
Expand Down Expand Up @@ -171,33 +176,36 @@ private void configureBindings() {

drivetrain.applyRequest(
() -> {
// return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(
// driverDesiredSpeeds);
// return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(
// driverDesiredSpeeds);
return drivetrain.driveDriverRelative(driverVelocitySupplier.get());
}));

// drive.withVelocityX(-leftDriveController.getYAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward)
// .withVelocityY(-leftDriveController.getXAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y
// (forward)
// .withVelocityY(-leftDriveController.getXAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left)
// .withRotationalRate(-rightDriveController.getXAxis().get() *
// GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left)
// .withRotationalRate(-rightDriveController.getXAxis().get() *
// GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative
// X (left)

// operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake));
// operatorController.getA().onTrue(new alignToTargetX(drivetrain, vision, 10, 0));
// operatorController.getA().onTrue(new alignToTargetX(drivetrain, vision, 10,
// 0));

// operatorController
// .getA()
// .onTrue(
// new AlignToAngle(
// drivetrain,
// new Rotation2d(),
// true,
// leftJoystickVelocityX,
// leftJoystickVelocityY)
// .andThen(
// new alignToTargetX(
// drivetrain, vision, 10, 0,
// .getA()
// .onTrue(
// new AlignToAngle(
// drivetrain,
// new Rotation2d(),
// true,
// leftJoystickVelocityX,
// leftJoystickVelocityY)
// .andThen(
// new alignToTargetX(
// drivetrain, vision, 10, 0,
// leftJoystickVelocityX)));

// operatorController.getA().toggleOnTrue(alignToReef(9, 0));
Expand All @@ -206,26 +214,43 @@ private void configureBindings() {
// leftDriveController.getLeftThumb().whileTrue(alignToReef(9, -0.4));
// leftDriveController.getBottomThumb().whileTrue(alignAndDriveToReef(19, 0));
// operatorController
// .getB()
// .whileTrue(
// drivetrain.applyRequest(
// () ->
// point.withModuleDirection(
// new Rotation2d(
// -operatorController.getLeftYAxis().get(),
// -operatorController
// .getLeftXAxis()
// .get()))));
// .getB()
// .whileTrue(
// drivetrain.applyRequest(
// () ->
// point.withModuleDirection(
// new Rotation2d(
// -operatorController.getLeftYAxis().get(),
// -operatorController
// .getLeftXAxis()
// .get()))));

// leftDriveController
// .getTrigger()
// .whileTrue(
// new WheelRadiusCharacterization(
// WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));
// .getTrigger()
// .whileTrue(
// new WheelRadiusCharacterization(
// WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));

// Run SysId routines when holding back/start and X/Y.
// Note that each routine should be run exactly once in a single log.

// operatorController
// .getBack()
// .and(operatorController.getY())
// .whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
// operatorController
// .getBack()
// .and(operatorController.getX())
// .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
// operatorController
// .getStart()
// .and(operatorController.getY())
// .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
// operatorController
// .getStart()
// .and(operatorController.getX())
// .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));

SmartDashboard.putData(
drivetrain
.sysIdDynamic(Direction.kForward)
Expand Down Expand Up @@ -262,16 +287,17 @@ private void configureBindings() {

// reset the field-centric heading on left bumper press
// operatorController
// .getLeftBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
// .getLeftBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
// operatorController
// .getRightBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(Pose2d.kZero)));
// .getRightBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(Pose2d.kZero)));

// Operator Mode Setting
operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode());
operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode());
operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode());
leftDriveController.getRightTopRight().onTrue(stateManager.setArmWristMode());
operatorController
.getLeftJoystick()
.toggleOnTrue(
Expand All @@ -290,17 +316,23 @@ private void configureBindings() {
// Coral Mode Bindings
final Trigger CORAL = stateManager.LEFT_CORAL.or(stateManager.RIGHT_CORAL);
final Trigger ALGAE = stateManager.ALGAE;
final Trigger ARMWRIST = stateManager.ARMWRIST;
ARMWRIST.and(operatorController.getY()).whileTrue(armSubsystem.armPivotUp());
ARMWRIST.and(operatorController.getA()).whileTrue(armSubsystem.armpivotDown());
ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft());
ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight());

CORAL.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4));
CORAL.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3));
CORAL.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2));
CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1));
CORAL.and(operatorController.getStart())
.onTrue(stateManager.moveToPosition(Position.Source));

CORAL.and(operatorController.getDPadDown())
.onTrue(stateManager.moveToPosition(Position.Home));
CORAL.and(operatorController.getDPadUp())
.onTrue(stateManager.moveToPosition(Position.Handoff));
CORAL.and(operatorController.getBack()).onTrue(Commands.none());

ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4Algae));
ALGAE.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3Algae));
Expand All @@ -316,7 +348,8 @@ private void configureBindings() {
.onTrue(stateManager.moveToPosition(Position.Quick34));
ALGAE.and(operatorController.getDPadRight())
.onTrue(stateManager.moveToPosition(Position.Quick23));
ALGAE.and(operatorController.getBack()).onTrue(Commands.none());

operatorController.getBack().onTrue(wristSubsystem.flipWristPosition());

// Driver Align Bindings, for a different/later day
// CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0));
Expand Down Expand Up @@ -377,6 +410,9 @@ private void configureBindings() {
leftDriveController.getLeftTopLeft().whileTrue(gripperSubsystem.gripperTuneable());

leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand());

// test
// operatorController.get
}

private double deadband(double value, double deadband) {
Expand Down Expand Up @@ -457,8 +493,4 @@ public Command alignToPiece() {
return new AlignToPiece(
drivetrain, driverVelocitySupplier, 0, piecePositionSupplier, Rotation2d.kZero);
}

public boolean getVerticality() {
return vision.isCoralVertical(0);
}
}
57 changes: 32 additions & 25 deletions src/main/java/frc/robot/constants/ArmConstants.java
View file
Original file line number Diff line number Diff line change
@@ -1,32 +1,39 @@
package frc.robot.constants;

import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.signals.GravityTypeValue;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;

public class ArmConstants {

public static final int ARM_PIVOT_MOTOR_ID = 0; // not correct motor ID
public static final int WRIST_MOTOR_ID = 1; // not correct motor ID

public static final Slot0Configs slot0Configs =
new Slot0Configs()
.withKA(0)
.withKD(0)
.withKG(0)
.withKI(0)
.withKP(0)
.withKS(0)
.withKV(0)
.withGravityType(GravityTypeValue.Elevator_Static);
;
public static final MotionMagicConfigs motionMagicConfigs =
new MotionMagicConfigs()
.withMotionMagicAcceleration(4) // these are guesses, come back here
.withMotionMagicCruiseVelocity(4) // also guess
.withMotionMagicJerk(4);

public static final double WRIST_KP = 0;
public static final double WRIST_KD = 0;
public static final double WRIST_KI = 0;
public static final String ARM_PIVOT_CANBUS = "CANivore";

public static final int ARM_THROUGHBORE_ENCODER_ID = 0;

// public static final Slot0Configs slot0Configs =
// new Slot0Configs()
// .withKA(0)
// .withKD(0)
// .withKG(0)
// .withKI(0)
// .withKP(0)
// .withKS(0)
// .withKV(0)
// .withGravityType(GravityTypeValue.Elevator_Static);
// ;
// public static final MotionMagicConfigs motionMagicConfigs =
// new MotionMagicConfigs()
// .withMotionMagicAcceleration(4) // these are guesses, come back here
// .withMotionMagicCruiseVelocity(4) // also guess
// .withMotionMagicJerk(4);

public static final CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs();

public static final double ARM_KP = 5;
public static final double ARM_KD = 0;
public static final double ARM_KI = 0;

public static final double ARM_TOLERANCE = 0.01;

public static final double upperLimit = 100;
public static final double lowerLimit = 0;
}
20 changes: 20 additions & 0 deletions src/main/java/frc/robot/constants/WristConstants.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
package frc.robot.constants;

public class WristConstants {

public static final int WRIST_MOTOR_ID = 11;
public static final String WRIST_CANBUS = "CANivore";

public static final int WRIST_THROUGHBORE_ENCODER_ID = 1;

public static final double WRIST_KP = 5;
public static final double WRIST_KD = 0;
public static final double WRIST_KI = 0;

public static final double WRIST_TOLERANCE = 0.01;

public static final double WristCurrent = 0;

public static final double upperLimit = 100;
public static final double lowerLimit = 0;
}
Loading

0 comments on commit e106f80

Please sign in to comment.