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

Commit

Permalink
Merge branch 'pivot2'
Browse files Browse the repository at this point in the history
  • Loading branch information
@carokhan
carokhan committed Oct 11, 2024
2 parents 201f06c + 00c6400 commit eaeddbe
Show file tree
Hide file tree
Showing 12 changed files with 231 additions and 213 deletions.
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,5 +26,6 @@
},
],
"java.test.defaultConfig": "WPIlibUnitTests",
"java.compile.nullAnalysis.mode": "automatic"
"java.compile.nullAnalysis.mode": "automatic",
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
}
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/BuildConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "Forte-2-5";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 52;
public static final String GIT_SHA = "5a5ec9f0498623c015059646a53857928b1ba114";
public static final String GIT_DATE = "2024-09-28 08:07:47 EDT";
public static final String GIT_BRANCH = "dev";
public static final String BUILD_DATE = "2024-09-28 12:45:11 EDT";
public static final long BUILD_UNIX_TIME = 1727541911180L;
public static final int GIT_REVISION = 57;
public static final String GIT_SHA = "3bce2b6cdf6012df5e1c4645e0fb0e2b16b02ffd";
public static final String GIT_DATE = "2024-10-10 17:55:55 EDT";
public static final String GIT_BRANCH = "pivot2";
public static final String BUILD_DATE = "2024-10-10 22:49:28 EDT";
public static final long BUILD_UNIX_TIME = 1728614968375L;
public static final int DIRTY = 1;

private BuildConstants(){}
Expand Down
26 changes: 12 additions & 14 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
package frc.robot;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -26,10 +27,6 @@
public final class Constants {
public static final double loopPeriodSecs = 0.02;

public static final double LOOP_TIME = 0.13;
public static final double ROBOT_MASS = 49.8951607;


public static final Mode currentMode = Mode.REAL;

public static enum Mode {
Expand Down Expand Up @@ -262,26 +259,30 @@ public static class FeederConstants {

public static class ShooterConstants {
public static final double pivotRatio = 496 / 3;
public static final double pivotMOI = 0.0022842632;
public static final double pivotLength = Units.inchesToMeters(7.01793315);
public static final double pivotLength = Units.inchesToMeters(7.5);
public static final double pivotMass = Units.lbsToKilograms(23);
public static final double pivotMOI = SingleJointedArmSim.estimateMOI(pivotLength, pivotMass);
// public static final double pivotMOI = .0001;

public static final double maxPivotVelocity = 10.5819313;
public static final double maxPivotVelocity = 20;
public static final double maxPivotAccel = 5;

public static final double pivotAbsConversion = Math.PI * 2.0;
public static final double pivotEncConversion = 2.0 * Math.PI / pivotRatio;
public static final double pivotOffset = 0.0;
public static final double simOffset = 0.19;
public static final double pivotTolerance = 0.01;
public static final double stallTimeout = 0.0;

public static final double down = 0.01;
public static final double up = down + Math.PI/4;

public static final double shooterMOI = 0.00920287973;

public static final int pivotCurrentLimit = 20;
public static final int pivotCurrentLimit = 25;

public static final double kGPivot = 0.00;
public static final double kVPivot = 0;
public static final double kGPivot = 0.381640625;
public static final double kVPivot = 0.875;
public static final double kAPivot = 0.00;

public static final double kVShooter = 0.0055; // COMP: 0.0055, DEMO: 0.0041875
Expand All @@ -291,14 +292,11 @@ public static class ShooterConstants {
public static final double kIShooterReal = 0.00000;
public static final double kSShooterReal = 0;

public static final double kPPivotReal = 1;
public static final double kPPivotSim = 100.0;
public static final double kPPivot = 5.0;

public static final double kPShooterSim = 0.5;
public static final double kIShooterSim = 0.0;
public static final double kSShooterSim = 0.5;

public static final double kPPivotReplay = 0.0;

public static final double kPShooterReplay = 0.0;
public static final double kIShooterReplay = 0.0;
Expand Down
99 changes: 34 additions & 65 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,12 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.RobotMap;
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.climber.Climb;
import frc.robot.subsystems.climber.ClimbIOReplay;
import frc.robot.subsystems.climber.ClimbIOSim;
Expand All @@ -45,10 +47,10 @@
import frc.robot.subsystems.intake.IntakeIOReplay;
import frc.robot.subsystems.intake.IntakeIOSim;
import frc.robot.subsystems.intake.IntakeIOSparkMax;
import frc.robot.subsystems.pivot.Pivot;
import frc.robot.subsystems.pivot.PivotIOReplay;
import frc.robot.subsystems.pivot.PivotIOSim;
import frc.robot.subsystems.pivot.PivotIOSparkMax;
import frc.robot.subsystems.pivot2.Pivot;
import frc.robot.subsystems.pivot2.PivotIOReplay;
import frc.robot.subsystems.pivot2.PivotIOSim;
import frc.robot.subsystems.pivot2.PivotIOSparkMax;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.ShooterIOReplay;
import frc.robot.subsystems.shooter.ShooterIOSim;
Expand Down Expand Up @@ -191,7 +193,7 @@ private void configureButtonBindings() {
m_operator.povUp().whileTrue(
Commands.parallel(
m_intake.setIntakeDown(false),
m_feeder.setRPM(() -> 3000))
m_feeder.setRPM(() -> 2500))
.until(() -> m_feeder.feederBeambreakObstructed()))
.onFalse(m_intake.setIntakeUp());

Expand All @@ -202,93 +204,60 @@ private void configureButtonBindings() {
m_feeder.setRPM(() -> -3000)))
.onFalse(m_intake.setIntakeUp());

// Right trigger for run intake forward
m_operator.rightTrigger(0.1).whileTrue(
Commands.parallel(
m_intake.setRollerRPM(() -> 3000),
m_feeder.setRPM(() -> 3000))
.until(() -> m_feeder.feederBeambreakObstructed()))
.onFalse(
Commands.parallel(
m_intake.setRollerVoltage(() -> 0),
m_feeder.setVoltage(() -> 0)).andThen(
Commands.parallel(
m_feeder.setRPM(() -> -1000),
m_intake.setRollerRPM(
() -> -1000),
m_shooter.setRPM(
() -> -1000,
1))
.until(() -> !m_feeder
.shooterBeambreakObstructed())
.unless(() -> !m_feeder
.shooterBeambreakObstructed())));

// Right bumper for run intake backward
m_operator.rightBumper().whileTrue(
Commands.parallel(
m_intake.setRollerRPM(() -> -3000),
m_feeder.setRPM(() -> -3000)))
.onFalse(
Commands.parallel(
m_intake.setRollerVoltage(() -> 0),
m_feeder.setVoltage(() -> 0)));

// Y for shooter at subwoofer
m_operator.y().whileTrue(
Commands.parallel(
m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)),
m_shooter.setRPM(() -> 5500, 0.3)).until(() -> m_shooter.atSetpoint()) // DEMO: 2000
m_shooter.setRPM(() -> 5800, 0.3)).until(() -> m_shooter.atSetpoint())
.andThen(m_feeder.setRPM(() -> 3000)
.until(() -> (!m_feeder.feederBeambreakObstructed()
&& !m_feeder.shooterBeambreakObstructed()))));

// X for shooter at amp

m_operator.leftBumper().whileTrue(
Commands.parallel(
m_feeder.setVoltage(() -> 0),
m_shooter.stopShooter(),
Commands.sequence(m_pivot.setPivotVoltage(() -> -1)))
.until(() -> m_pivot.atSetpoint()
|| m_pivot.isStalled())
.andThen(
Commands.either(m_pivot.resetEncoder(),
m_pivot.runZero(),
() -> m_pivot.isStalled()).andThen(m_pivot.setPivotTarget(() -> m_pivot.getAngleRadians()).andThen(
m_pivot.setPivotVoltage(() -> 0))
)));


// m_operator.rightBumper().whileTrue(
// m_pivot.runCurrentZeroing()
// );

// B for shooter at podium or feeding
m_operator.b().whileTrue(
Commands.parallel(
m_pivot.setPivotTarget(() -> Units.degreesToRadians(20)),
m_shooter.setRPM(() -> 5500, 1)).until(() -> m_shooter.atSetpoint())); // DEMO: 2000, COMP: 5500
m_shooter.setRPM(() -> 5800, 1.0)).until(() -> m_shooter.atSetpoint())
.andThen(m_feeder.setRPM(() -> 2000)
.until(() -> (!m_feeder.feederBeambreakObstructed()
&& !m_feeder.shooterBeambreakObstructed()))));

// A for shooter at source
m_operator.a().whileTrue(
Commands.parallel(
m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)),
m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)),
m_shooter.setRPM(() -> -1500, 1.0),
m_feeder.setRPM(() -> -1500),
m_intake.setRollerRPM(() -> -1000))
.until(() -> (m_feeder.feederBeambreakObstructed()
&& !m_feeder.shooterBeambreakObstructed()))
.andThen(Commands.parallel(
m_shooter.stopShooter(),
.andThen(Commands.parallel(m_shooter.stopShooter(),
m_pivot.setPivotTarget(
() -> Units.degreesToRadians(0))),
m_feeder.setVoltage(() -> 0))
.andThen(
Commands.parallel(
m_feeder.setRPM(() -> 1000),
m_intake.setRollerRPM(() -> 1000))
.until(() -> !m_feeder
.shooterBeambreakObstructed())
.unless(() -> !m_feeder
.shooterBeambreakObstructed())));

// Left bumper for reset shooter
m_operator.leftBumper().whileTrue(
Commands.parallel(
m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)),
m_feeder.setVoltage(() -> 0),
m_shooter.stopShooter()
)
);
() -> Units.degreesToRadians(0.0)))));

// Left trigger for shoot
m_operator.leftTrigger(0.1).onTrue(
m_shooter.setRPM(() -> 5500, 0.3)) // DEMO: 3000, COMP: 5500
Commands.parallel(m_shooter.setRPM(() -> 5800, 0.3),
m_feeder.setRPM(() -> 2000),
m_intake.setRollerRPM(() -> 2000)))
.onFalse(m_shooter.stopShooter()
.andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0))));

Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/Visualizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.climber.Climb;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.pivot.Pivot;
import frc.robot.subsystems.pivot2.Pivot;

public class Visualizer extends SubsystemBase {
private Mechanism2d m_main;
Expand All @@ -28,6 +28,7 @@ public class Visualizer extends SubsystemBase {
private MechanismLigament2d m_climberTarget;
private MechanismLigament2d m_intakeTarget;
private MechanismLigament2d m_pivotTarget;
private MechanismLigament2d m_pivotRelative;

private MechanismRoot2d m_climberRoot;
private MechanismRoot2d m_intakeRoot;
Expand Down Expand Up @@ -57,6 +58,7 @@ public Visualizer(Climb climber, Intake intake, Pivot pivot) {
m_intakeTarget = m_intakeRoot.append(new MechanismLigament2d("Intake Target", Units.inchesToMeters(14.914264), 83.649627, 2, new Color8Bit(Color.kBlue)));

m_pivotMech = m_pivotRoot.append(new MechanismLigament2d("Shooter", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kWhite)));
m_pivotRelative = m_pivotRoot.append(new MechanismLigament2d("Shooter Relative", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kGray)));
m_pivotTarget = m_pivotRoot.append(new MechanismLigament2d("Shooter Target", Units.inchesToMeters(13.1001837), 12, 2, new Color8Bit(Color.kBeige)));

SmartDashboard.putData("Climb Up", (Sendable) m_climber.setExtensionCmd(() -> ClimbConstants.maxHeight));
Expand All @@ -80,6 +82,7 @@ public void periodic() {
m_intakeTarget.setAngle(Units.radiansToDegrees(m_intake.getTargetRadians()));

m_pivotMech.setAngle(Units.radiansToDegrees(m_pivot.getAngleRadians()));
m_pivotRelative.setAngle(Units.radiansToDegrees(m_pivot.getRelativeRadians()));
m_pivotTarget.setAngle(Units.radiansToDegrees(m_pivot.getTargetRadians()));

Logger.recordOutput("Visualizer/FullRobot", m_main);
Expand Down
76 changes: 0 additions & 76 deletions src/main/java/frc/robot/subsystems/pivot/Pivot.java
View file

This file was deleted.

Loading

0 comments on commit eaeddbe

Please sign in to comment.