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

Commit

Permalink
Merge pull request #25 from Gongoliers/2_23_24_intake
Browse files Browse the repository at this point in the history 
2/23/24 intake
  • Loading branch information
@haydenheroux
haydenheroux authored Feb 25, 2024
2 parents d662100 + b9071db commit 3dd6337
Show file tree
Hide file tree
Showing 16 changed files with 307 additions and 198 deletions.
21 changes: 16 additions & 5 deletions simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,30 +41,41 @@
"transitory": {
"Shuffleboard": {
"Arm": {
"Derivatives": {
"open": true
},
"Feedforward": {
"open": true
},
"Goal": {
"open": true
},
"Position": {
"open": true
},
"Setpoint": {
"open": true
},
"Velocities": {
"open": true
}
},
"Voltages": {
"open": true
},
"open": true
},
"Climber": {
"Elevator": {
"open": true
},
"Linear Actuator": {
"open": true
},
"open": true
}
},
"Intake": {
"Roller": {
"open": true
},
"open": true
}
},
"Odometry": {
"Position": {
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/RobotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,5 @@ public enum Subsystem {
VISION
}

public static final Set<Subsystem> REAL_SUBSYSTEMS =
EnumSet.of(Subsystem.INTAKE, Subsystem.SHOOTER);
public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.INTAKE);
}
34 changes: 18 additions & 16 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,15 @@

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.lib.Telemetry;
import frc.robot.arm.Arm;
import frc.robot.arm.ArmState;
import frc.robot.auto.Auto;
import frc.robot.climber.Climber;
import frc.robot.intake.Intake;
import frc.robot.intake.IntakeConstants.PivotMotorConstants;
import frc.robot.lights.Lights;
import frc.robot.odometry.Odometry;
import frc.robot.shooter.Shooter;
Expand Down Expand Up @@ -68,25 +71,24 @@ private void configureBindings() {
driverController.y().onTrue(odometry.tare());
driverController.x().whileTrue(swerve.cross());

// operatorController
// .leftTrigger()
// .whileTrue(
// Commands.parallel(arm.to(ArmState.INTAKE), intake.out())
// .andThen(Commands.parallel(intake.intake(),
// shooter.intake()))).onFalse(Commands.runOnce(() ->
// intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE)));
Command toIntake = Commands.parallel(
Commands.waitUntil(intake::isNotStowed).andThen(arm.to(ArmState.INTAKE)),
intake.out());

// operatorController
// .rightTrigger()
// .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot()));
Command toStow = Commands.runOnce(() -> {
arm.setGoal(ArmState.STOW);
intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE);
});

// operatorController.rightBumper().whileTrue(shooter.shoot());
operatorController
.leftTrigger()
.whileTrue(
toIntake
.andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(toStow);

// operatorController.a().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.AMP)));
// operatorController.b().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));

operatorController.a().whileTrue(intake.in());
operatorController.b().whileTrue(intake.out());
operatorController
.rightTrigger()
.whileTrue(arm.to(ArmState.SHOOT).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
}

/**
Expand Down
15 changes: 7 additions & 8 deletions src/main/java/frc/robot/RobotMechanisms.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import frc.robot.arm.ArmConstants.ElbowMotorConstants;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
import frc.robot.arm.ArmConstants.WristMotorConstants;
import frc.robot.arm.ArmState;
import frc.robot.intake.IntakeConstants.PivotMotorConstants;

Expand All @@ -20,7 +20,7 @@ public class RobotMechanisms {

private final Mechanism2d mechanism;

private MechanismLigament2d shoulder, elbow, intake;
private MechanismLigament2d shoulder, wrist, intake;

private final double WIDTH =
2
Expand Down Expand Up @@ -55,11 +55,11 @@ private void initializeArmMechanism() {
0,
armThickness,
new Color8Bit(Color.kOrange)));
elbow =
wrist =
shoulder.append(
new MechanismLigament2d(
"elbow",
ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(),
"wrist",
WristMotorConstants.JOINT_CONSTANTS.lengthMeters(),
0,
armThickness,
new Color8Bit(Color.kGreen)));
Expand Down Expand Up @@ -127,11 +127,10 @@ public Mechanism2d getMechanism() {

public void setArmState(ArmState state) {
Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position);
Rotation2d elbowRotation = Rotation2d.fromRotations(state.elbow().position);
// Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);

shoulder.setAngle(shoulderRotation);
elbow.setAngle(elbowRotation.minus(shoulderRotation));
wrist.setAngle(wristRotation.plus(Rotation2d.fromDegrees(90)));
}

public void setIntakeAngle(Rotation2d angle) {
Expand Down
72 changes: 24 additions & 48 deletions src/main/java/frc/robot/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,8 @@
import edu.wpi.first.wpilibj2.command.Commands;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
import frc.lib.TwoJointedArmFeedforward;
import frc.lib.TwoJointedArmFeedforward.TwoJointedArmFeedforwardResult;
import frc.robot.RobotMechanisms;
import frc.robot.arm.ArmConstants.ElbowMotorConstants;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues;
import frc.robot.arm.WristMotorIO.WristMotorIOValues;
import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;

/** Subsystem class for the arm subsystem. */
Expand All @@ -28,29 +24,28 @@ public class Arm extends Subsystem {
/** Shoulder motor values. */
private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues();

/** Elbow motor. */
private final ElbowMotorIO elbowMotor;
/** Wrist motor. */
private final WristMotorIO wristMotor;

/** Elbow motor values. */
private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
/** Wrist motor values. */
private final WristMotorIOValues wristMotorValues = new WristMotorIOValues();

private ArmState goal, setpoint;
private final ArmState initialState = ArmState.STOW;

private final TwoJointedArmFeedforward feedforward;
private ArmState goal, setpoint;

/** Creates a new instance of the arm subsystem. */
private Arm() {
shoulderMotor = ArmFactory.createShoulderMotor();
elbowMotor = ArmFactory.createElbowMotor();
wristMotor = ArmFactory.createWristMotor();

setPosition(ArmState.STOW.withElbow(Rotation2d.fromDegrees(0)));
shoulderMotor.configure();
wristMotor.configure();

goal = getPosition();
setpoint = getPosition();
setPosition(initialState);

feedforward =
new TwoJointedArmFeedforward(
ShoulderMotorConstants.JOINT_CONSTANTS, ElbowMotorConstants.JOINT_CONSTANTS);
goal = initialState.withWrist(ArmState.STOW.wrist());
setpoint = getPosition();
}

/**
Expand All @@ -69,8 +64,9 @@ public static Arm getInstance() {
@Override
public void periodic() {
shoulderMotor.update(shoulderMotorValues);
wristMotor.update(wristMotorValues);

setSetpoint(getSetpoint().nextSetpoint(goal));
setSetpoint(setpoint.nextSetpoint(goal));

RobotMechanisms.getInstance().setArmState(getPosition());
}
Expand All @@ -83,15 +79,13 @@ public void addToShuffleboard(ShuffleboardTab tab) {
"Shoulder Position (deg)",
() -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
position.addDouble(
"Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations));
"Wrist Position (deg)", () -> Units.rotationsToDegrees(wristMotorValues.positionRotations));

ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");

setpoint.addDouble(
"Shoulder Setpoint (deg)",
() -> Units.rotationsToDegrees(getSetpoint().shoulder().position));
setpoint.addDouble(
"Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().elbow().position));
setpoint.addDouble(
"Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().wrist().position));
setpoint.addBoolean("At Setpoint?", this::atSetpoint);
Expand All @@ -100,16 +94,14 @@ public void addToShuffleboard(ShuffleboardTab tab) {

goal.addDouble(
"Shoulder Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().shoulder().position));
goal.addDouble(
"Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().elbow().position));
goal.addDouble(
"Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().wrist().position));
goal.addBoolean("At Goal?", this::atGoal);

ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages");

voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.appliedVolts);
voltages.addDouble("Elbow Voltage (V)", () -> elbowMotorValues.appliedVolts);
voltages.addDouble("Wrist Voltage (V)", () -> wristMotorValues.appliedVolts);

ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives");

Expand All @@ -119,21 +111,15 @@ public void addToShuffleboard(ShuffleboardTab tab) {
"Shoulder Acceleration (rpsps)",
() -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond);
derivatives.addDouble(
"Elbow Velocity (rps)", () -> elbowMotorValues.velocityRotationsPerSecond);
"Wrist Velocity (rps)", () -> wristMotorValues.velocityRotationsPerSecond);
derivatives.addDouble(
"Elbow Acceleration (rpsps)",
() -> elbowMotorValues.accelerationRotationsPerSecondPerSecond);

ShuffleboardLayout feedforward = Telemetry.addColumn(tab, "Feedforward");

feedforward.addDouble("Shoulder Feedforward (V)", () -> calculateFeedforward().shoulderVolts());
feedforward.addDouble("Elbow Feedforward (V)", () -> calculateFeedforward().elbowVolts());
"Wrist Acceleration (rpsps)",
() -> wristMotorValues.accelerationRotationsPerSecondPerSecond);
}

public void setPosition(ArmState state) {
shoulderMotor.setPosition(state.shoulder().position);
elbowMotor.setPosition(state.elbow().position);
// wristMotor.setPosition(state.wrist().getRotations());
wristMotor.setPosition(state.wrist().position);
}

/**
Expand All @@ -143,12 +129,11 @@ public void setPosition(ArmState state) {
*/
public ArmState getPosition() {
shoulderMotor.update(shoulderMotorValues);
elbowMotor.update(elbowMotorValues);
wristMotor.update(wristMotorValues);

return new ArmState(
Rotation2d.fromRotations(shoulderMotorValues.positionRotations),
Rotation2d.fromRotations(elbowMotorValues.positionRotations),
Rotation2d.fromRotations(0));
Rotation2d.fromRotations(wristMotorValues.positionRotations));
}

public ArmState getGoal() {
Expand All @@ -175,19 +160,10 @@ private void setSetpoint(ArmState setpoint) {
this.setpoint = setpoint;

shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity);
elbowMotor.setSetpoint(setpoint.elbow().position, setpoint.elbow().velocity);
// wristMotor.runSetpoint(state.wrist().getRotations());
wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity);
}

public Command to(ArmState goal) {
return runOnce(() -> setGoal(goal)).andThen(Commands.waitUntil(this::atGoal));
}

public TwoJointedArmFeedforwardResult calculateFeedforward() {
ArmState position = getPosition();

return feedforward.calculateFeedforward(
Rotation2d.fromRotations(position.shoulder().position),
Rotation2d.fromRotations(position.elbow().position));
}
}
Loading

0 comments on commit 3dd6337

Please sign in to comment.