Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #19 from Team4388/Climber

Browse Source 
Climber
This commit is contained in:
Ryan Manley
2022-03-18 13:04:50 -06:00
committed by GitHub
parent fbd74f72ce 1f357082b9
commit 556bbb32b4
9 changed files with 766 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+67
View File
@@ -33,6 +33,8 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity. * constants are needed, to reduce verbosity.
*/ */
public final class Constants { public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final class SwerveDriveConstants { public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4.0; public static final double ROTATION_SPEED = 4.0;
public static final double WIDTH = 23.5; public static final double WIDTH = 23.5;
@@ -167,6 +169,71 @@ public final class Constants {
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
} }
public static final class ClimberConstants {
public static final int SHOULDER_ID = 30;
public static final int ELBOW_ID = 31;
public static final int GYRO_ID = 14;
// TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
public static final double LOWER_ARM_LENGTH = 27;
public static final double MAX_ARM_LENGTH = 53;
public static final double MIN_ARM_LENGTH = 1;
public static final double MOVE_SPEED = .1; // cm per second
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
public static final double SHOULDER_MAX_ANGLE = 135;
public static final double ELBOW_MAX_ANGLE = 180;
public static final double ELBOW_GB_RATIO = 1.d;
public static final double SHOULDER_GB_RATIO = 1.d;
public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0;
public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
public static final double ELBOW_SOFT_LIMIT_FORWARD = 0;
public static final double ELBOW_SOFT_LIMIT_REVERSE = 0;
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 1;
public static final int ELBOW_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 1;
public static final Gains SHOULDER_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0);
public static final int CLIMBER_TIMEOUT_MS = 50;
public static final double THRESHOLD = 0;
// TODO: Update Constants
// Robot Angle
public static final double ROBOT_ANGLE_ID = 0;
}
public static final class ClawConstants {
public static final int LEFT_CLAW_ID = 44;
public static final int RIGHT_CLAW_ID = 45;
public static final double OPEN_POSITION = 0; // TODO: find actual position
public static final double CLOSE_POSITION = 1; // TODO: find actual position
public static final double THRESHOLD = .1; // TODO: find actual threshold
public static final double CALIBRATION_SPEED = 0;
public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit;
public static final int TOP_LIMIT = 1800;
public static final int BOTTOM_LIMIT = 1200;
}
/** /**
* The OIConstants class contains the ID for the XBox controllers * The OIConstants class contains the ID for the XBox controllers
*/ */
src/main/java/frc4388/robot/Robot.java
+8 -1
View File
@@ -163,6 +163,11 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work. // block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run(); CommandScheduler.getInstance().run();
// print odometry data to smart dashboard for debugging (if causing timeout
// errors, you can comment it)
// SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
// SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
// SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
// odo chooser stuff // odo chooser stuff
addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)),
new Pose2d(1, 2, new Rotation2d(Math.PI/3)), new Pose2d(1, 2, new Rotation2d(Math.PI/3)),
@@ -198,7 +203,7 @@ public class Robot extends TimedRobot {
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
.resolve("recording." + System.currentTimeMillis() + ".path").toFile(); .resolve("recording." + System.currentTimeMillis() + ".path").toFile();
if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
m_robotContainer.createPath(null, null, false).write(outputFile); // m_robotContainer.createPath(null, null, false).write(outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
} else } else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
@@ -247,6 +252,7 @@ public class Robot extends TimedRobot {
public void teleopInit() { public void teleopInit() {
LOGGER.fine("teleopInit()"); LOGGER.fine("teleopInit()");
Robot.alliance = DriverStation.getAlliance(); Robot.alliance = DriverStation.getAlliance();
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
@@ -257,6 +263,7 @@ public class Robot extends TimedRobot {
} }
m_robotContainer.resetOdometry(selectedOdo); m_robotContainer.resetOdometry(selectedOdo);
// This makes sure that the autonomous stops running when // This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to // teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove // continue until interrupted by another command, remove
src/main/java/frc4388/robot/RobotContainer.java
+85 -129
View File
@@ -59,6 +59,14 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.commands.RunClaw;
import frc4388.robot.subsystems.ClimberRewrite;
import frc4388.robot.subsystems.Claws.ClawType;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.StorageConstants;
@@ -82,6 +90,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.VisionOdometry; import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.LEDPatterns; import frc4388.utility.LEDPatterns;
import frc4388.utility.ListeningSendableChooser; import frc4388.utility.ListeningSendableChooser;
import frc4388.utility.PathPlannerUtil; import frc4388.utility.PathPlannerUtil;
@@ -103,6 +112,11 @@ public class RobotContainer {
// RobotMap // RobotMap
public final RobotMap m_robotMap = new RobotMap(); public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
// Subsystems // Subsystems
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
@@ -118,7 +132,7 @@ public class RobotContainer {
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
// Controllers // Controllers
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -127,7 +141,7 @@ public class RobotContainer {
// Autonomous // Autonomous
private PathPlannerTrajectory loadedPathTrajectory = null; private PathPlannerTrajectory loadedPathTrajectory = null;
private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath); // private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
private final List<Waypoint> pathPoints = new ArrayList<>(); private final List<Waypoint> pathPoints = new ArrayList<>();
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
@@ -149,6 +163,17 @@ public class RobotContainer {
configureButtonBindings(); configureButtonBindings();
/* Default Commands */ /* Default Commands */
// moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.7, getOperatorController().getRightY() * 0.7),
m_robotClimber));
// IK command
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
// getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input // Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand( m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput( new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
@@ -208,6 +233,7 @@ public class RobotContainer {
private void configureButtonBindings() { private void configureButtonBindings() {
/* Driver Buttons */ /* Driver Buttons */
// Start > Calibrate Odometry // Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kBack.value) new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
@@ -215,74 +241,29 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kStart.value) new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
.whenPressed(m_robotSwerveDrive::resetGyro); .whenPressed(m_robotSwerveDrive::resetGyro);
// Left Bumper > Shift Down // Left Bumper > Shift Down
// new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// // Right Bumper > Shift Up // Right Bumper > Shift Up
// new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Axis.kLeftTrigger.value)
.whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.1), m_robotClimber))
.whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0)));
new JoystickButton(getDriverController(), XboxController.Axis.kRightTrigger.value)
.whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(-0.1), m_robotClimber))
.whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0)));
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025));
// new JoystickButton(getDriverController(), XboxController.Button.kB.value)
// .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025));
// .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2)))
// .whenReleased(new RunCommand(() -> testElbowMotor.set(0)));
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
// new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
// .whenPressed(() -> m_robotMap.leftFront.reset())
// .whenPressed(() -> m_robotMap.rightFront.reset())
// .whenPressed(() -> m_robotMap.leftBack.reset())
// .whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */ /* Operator Buttons */
// X > Extend Intake new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value) new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)); .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425)))
// .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475)))
// .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5)))
// .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
@@ -295,31 +276,11 @@ public class RobotContainer {
//Toggles extender in and out //Toggles extender in and out
new JoystickButton(getOperatorController(), XboxController.Button.kY.value) new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
// .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value) new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));\
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom));
// Right Bumper > Storage In // Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
@@ -359,10 +320,6 @@ public class RobotContainer {
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
// .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true)))
// .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false)));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
// .whileHeld(new TurretManual(m_robotTurret)); // .whileHeld(new TurretManual(m_robotTurret));
@@ -372,9 +329,6 @@ public class RobotContainer {
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
// .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); // .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whileHeld(new RunCommand(() -> m_robotTurret.turnOnLeftLimitSwitch(), m_robotTurret))
// .whenReleased(new RunCommand(() -> m_robotTurret.turnOffLeftLimitSwitch(), m_robotTurret));
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
@@ -391,26 +345,27 @@ public class RobotContainer {
* @return the command to run in autonomous * @return the command to run in autonomous
*/ */
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
if (loadedPathTrajectory != null) { // if (loadedPathTrajectory != null) {
PIDController xController = SwerveDriveConstants.X_CONTROLLER; // PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER; // PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI); // thetaController.enableContinuousInput(-Math.PI, Math.PI);
PathPlannerState initialState = loadedPathTrajectory.getInitialState(); // PathPlannerState initialState = loadedPathTrajectory.getInitialState();
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); // Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
return new SequentialCommandGroup( // return new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive.m_gyro::reset), // new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, // new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, // m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), // m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); // new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
} else { // } else {
LOGGER.severe("No auto selected."); // LOGGER.severe("No auto selected.");
return new RunCommand(() -> { // return new RunCommand(() -> {
}).withName("No Autonomous Path"); // }).withName("No Autonomous Path");
} // }
return null;
} }
public XboxController getDriverController() { public XboxController getDriverController() {
@@ -434,18 +389,18 @@ public class RobotContainer {
* *
* @return Odometry * @return Odometry
*/ */
public Pose2d getOdometry() { // public Pose2d getOdometry() {
return m_robotSwerveDrive.getOdometry(); // return m_robotSwerveDrive.getOdometry();
} // }
/** /**
* Set odometry to given pose. * Set odometry to given pose.
* *
* @param pose Pose to set odometry to. * @param pose Pose to set odometry to.
*/ */
public void resetOdometry(Pose2d pose) { // public void resetOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose); // m_robotSwerveDrive.resetOdometry(pose);
} // }
/** /**
* Creates a WatchKey for the path planner directory and registers it with the * Creates a WatchKey for the path planner directory and registers it with the
@@ -454,31 +409,32 @@ public class RobotContainer {
* latest path files. * latest path files.
* Finally, adds the existing path files to the auto chooser * Finally, adds the existing path files to the auto chooser
*/ */
private void autoInit() { // private void autoInit() {
try { // try {
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
StandardWatchEventKinds.ENTRY_DELETE); // StandardWatchEventKinds.ENTRY_DELETE);
// TODO: Store this and other commands as fields so they can be rescheduled. // // TODO: Store this and other commands as fields so they can be rescheduled.
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { // new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
@Override // @Override
public boolean runsWhenDisabled() { // public boolean runsWhenDisabled() {
return true; // return true;
} // }
}.withName("Path Watcher").schedule(); // }.withName("Path Watcher").schedule();
} catch (IOException exception) { // } catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); // LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
} // }
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) // Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
.filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
.forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
SmartDashboard.putData("Auto Chooser", autoChooser); // SmartDashboard.putData("Auto Chooser", autoChooser);
} // }
/** /**
* Creates a button on the SmartDashboard that will record the path of the * Creates a button on the SmartDashboard that will record the path of the
* robot. * robot.
*/ */
public void recordInit() { public void recordInit() {
SmartDashboard.putData("Recording", SmartDashboard.putData("Recording",
new RunCommand(this::recordPeriodic) { new RunCommand(this::recordPeriodic) {
src/main/java/frc4388/robot/RobotMap.java
+22 -5
View File
@@ -4,6 +4,12 @@
package frc4388.robot; package frc4388.robot;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
@@ -11,14 +17,16 @@ import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.CAN;
import edu.wpi.first.networktables.NetworkTableType; import edu.wpi.first.networktables.NetworkTableType;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.ShooterConstants;
@@ -49,7 +57,6 @@ public class RobotMap {
// void configureLEDMotorControllers() {} // void configureLEDMotorControllers() {}
/* Swerve Subsystem */ /* Swerve Subsystem */
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
@@ -188,6 +195,16 @@ public class RobotMap {
SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
} }
/* Climb Subsystem */
public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
/* Hooks Subsystem */
// public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
// public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
public final Servo leftClaw = new Servo(1); // TODO: find actual channel
public final Servo rightClaw = new Servo(2); // TODO: find actual channel
// Shooter Config // Shooter Config
/* Boom Boom Subsystem */ /* Boom Boom Subsystem */
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
@@ -247,12 +264,12 @@ public class RobotMap {
angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
angleAdjusterMotor.setInverted(true); angleAdjusterMotor.setInverted(true);
} }
/* Serializer Subsystem */ /* Serializer Subsystem */
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
/* Intake Subsytem */ /* Intake Subsystem */
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
src/main/java/frc4388/robot/commands/RunClaw.java
+53
View File
@@ -0,0 +1,53 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.subsystems.Claws;
public class RunClaw extends CommandBase {
// parameters
public Claws m_claws;
public Claws.ClawType clawType;
public boolean open;
/**
* Creates a new RunClaw, which runs a claw.
* @param sClaws Claws subsystem.
* @param which Which claw to run.
* @param open Whether to open or close the claw.
*/
public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) {
// Use addRequirements() here to declare subsystem dependencies.
m_claws = sClaws;
clawType = which;
this.open = open;
addRequirements(m_claws);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// m_claws.runClaw(clawType, open);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// return m_claws.checkSwitchAndCurrent(clawType);
return false; // TODO: real return
}
}
src/main/java/frc4388/robot/commands/climber/RunClimberPath.java
+74
View File
@@ -0,0 +1,74 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.climber;
import org.opencv.core.Point;
import edu.wpi.first.wpilibj.drive.Vector2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.subsystems.ClimberRewrite;
import frc4388.utility.Vector2D;
public class RunClimberPath extends CommandBase {
ClimberRewrite climber;
Claws claws;
Point[] path;
int nextIndex;
boolean endPath;
/** Creates a new RunClimberPath. */
public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) {
path = _path;
endPath = false;
climber = _climber;
claws = _claws;
addRequirements(climber, claws);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
claws.setOpen(true);
nextIndex = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// if(!claws.fullyOpen())
return;
// Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
// Vector2D dir = new Vector2D(path[nextIndex]);
// dir.subtract(new Vector2D(climberPos));
// if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
// nextIndex++;
// else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
// endPath = true;
// claws.setOpen(false);
// } else if(!endPath) {
// dir = dir.unit();
// climber.controlWithInput(dir.x, dir.y);
// }
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// return endPath && claws.fullyClosed();
return false;
}
}
src/main/java/frc4388/robot/subsystems/Claws.java
+171
View File
@@ -0,0 +1,171 @@
package frc4388.robot.subsystems;
import java.nio.file.ClosedWatchServiceException;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants;
public class Claws extends SubsystemBase {
public Servo m_leftClaw;
public Servo m_rightClaw;
// public CANSparkMax m_leftClaw;
// public CANSparkMax m_rightClaw;
// private SparkMaxLimitSwitch m_leftLimitSwitchF;
// private SparkMaxLimitSwitch m_rightLimitSwitchF;
// private SparkMaxLimitSwitch m_leftLimitSwitchR;
// private SparkMaxLimitSwitch m_rightLimitSwitchR;
private double m_leftOffset;
private double m_rightOffset;
private boolean m_open;
public static enum ClawType {LEFT, RIGHT}
public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
// m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
// m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchF.enableLimitSwitch(true);
// m_rightLimitSwitchF.enableLimitSwitch(true);
// m_leftLimitSwitchR.enableLimitSwitch(true);
// m_rightLimitSwitchR.enableLimitSwitch(true);
// m_leftClaw.setInverted(true);
// m_rightClaw.setInverted(true);
m_open = false;
// m_leftClaw.set(ClawConstants.CALIBRATION_SPEED);
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
}
/**
* Run a specific claw to open or close.
* @param which Which claw to run.
* @param open Whether to open or close the claw.
*/
// public void runClaw(ClawType which, boolean open) {
// int direction = open ? 1 : -1;
// if (which == Claws.ClawType.LEFT) {
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// // m_leftClaw.getEncoder().setPosition(setPos);
// m_leftClaw.setSpeed(direction * 0.1);
// } else if (which == Claws.ClawType.RIGHT) {
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// // m_rightClaw.getEncoder().setPosition(setPos);
// m_rightClaw.setSpeed(direction * 0.1);
// }
// m_open = open;
// }
// public void runClaw(ClawType which, double input) {
// if (which == Claws.ClawType.LEFT) {
// m_leftClaw.setSpeed(input);
// } else if (which == Claws.ClawType.RIGHT) {
// m_rightClaw.setSpeed(input);
// }
// }
// public void runClaws(double input)
// {
// m_leftClaw.setSpeed(input);
// m_rightClaw.setSpeed(input);
// }
/**
* Sets the state of both hooks
* @param open The state of the hooks
*/
public void setOpen(boolean open) {
SmartDashboard.putBoolean("open", open);
if(open) {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset);
// m_leftClaw.setPosition(.7);
// m_rightClaw.setPosition(.7);
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);//ClawConstants.OPEN_POSITION);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);//ClawConstants.OPEN_POSITION);
} else {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset);
// m_leftClaw.setPosition(.3);
// m_rightClaw.setPosition(.3);
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 100);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 700);
}
}
// public double[] getOffsets() {
// return new double[] {m_leftOffset, m_rightOffset};
// }
// public boolean fullyOpen() {
// return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
// Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
// public boolean fullyClosed() {
// return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
// Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
// }
/**
* Check if a limit switch is pressed or current limit exceeded for a claw.
* @param which Which claw to check.
* @param limit The current limit.
* @return Whether to interrupt the RunClaw command or not.
*/
// public boolean checkSwitchAndCurrent(ClawType which) {
// if (which == ClawType.LEFT) {
// if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// else if (which == ClawType.RIGHT) {
// if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// return false;
// }
// @Override
public void periodic() {
SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw());
SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw());
// if (fullyOpen() || fullyClosed()) {
// m_leftClaw.setSpeed(0.0);
// m_rightClaw.setSpeed(0.0);
// }
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
// m_leftOffset = m_leftClaw.getPosition();
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
// // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+285
View File
@@ -0,0 +1,285 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import java.util.HashMap;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import org.opencv.core.Point;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimberConstants;
public class ClimberRewrite extends SubsystemBase {
private static double[][] shoulderFeedMap;
private static double[][] elbowFeedMap;
public static void configureFeedMaps() {
}
private WPI_TalonFX m_shoulder;
private WPI_TalonFX m_elbow;
private WPI_Pigeon2 m_gyro;
private Point tPoint;
private boolean groundRelative;
/** Creates a new ClimberRewrite. */
public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
m_shoulder = shoulder;
m_shoulder.configFactoryDefault();
m_shoulder.setNeutralMode(NeutralMode.Brake);
m_elbow = elbow;
m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake);
// setClimberGains();
// shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
// elbowStartPosition = m_elbow.getSelectedSensorPosition();
m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
m_elbow.configForwardSoftLimitEnable(false);
m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
m_elbow.configReverseSoftLimitEnable(false);
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
m_shoulder.configForwardSoftLimitEnable(false);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
m_shoulder.configReverseSoftLimitEnable(false);
m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
m_shoulder.overrideLimitSwitchesEnable(true);
m_elbow.overrideLimitSwitchesEnable(true);
tPoint = new Point();
if(groundRelative)
m_gyro = gyro;
groundRelative = _groundRelative;
}
public void setClimberGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
}
public void setClimberFeedForward(double shoulderF, double elbowF) {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, shoulderF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, elbowF, ClimberConstants.CLIMBER_TIMEOUT_MS);
}
public void compensateFeedForArmAngles() {
double shoulderPosition = m_shoulder.getSelectedSensorPosition();
double elbowPosition = m_elbow.getSelectedSensorPosition();
double shoulderFeed = 0;
double elbowFeed = 0;
for(int i = 1; i < shoulderFeedMap.length; i++) {
if(shoulderFeedMap[i][0] > shoulderPosition) {
double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]);
double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1];
shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1];
}
}
for(int i = 1; i < elbowFeedMap.length; i++) {
if(elbowFeedMap[i][0] > elbowPosition) {
double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]);
double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1];
elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1];
}
}
setClimberFeedForward(shoulderFeed, elbowFeed);
}
public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput);
m_elbow.set(elbowOutput);
}
public double[] getJointAngles() {
return new double[] {
(m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO,
(m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO
};
}
public void setJointAngles(double[] angles) {
System.out.println(angles);
setJointAngles(angles[0], angles[1]);
}
public void setJointAngles(double shoulderAngle, double elbowAngle) {
shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// Convert radians to ticks
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO;
elbowPosition *= ClimberConstants.ELBOW_GB_RATIO;
// shoulderPosition += m_shoulderOffset;
// elbowPosition += m_elbowOffset;
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
}
public void controlWithInput(double xInput, double yInput) {
tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02;
tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02;
}
@Override
public void periodic() {
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
// setJointAngles(jointAngles);
}
/**
* Gets joint angles for climber arm
* {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the
* rotation of the robot. Both parameters should be in cm.
* Does not set the motors automatically
* <p><p>
* IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399
*
* @param targetPoint Target xy point for the climber arm to go to
* @param tiltAngle The tilt of the robot
* @return [shoulderAngle, elbowAngle] in radians */
public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) {
double [] angles = new double[2];
double mag = Math.hypot(targetPoint.x, targetPoint.y);
if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
mag = ClimberConstants.MAX_ARM_LENGTH;
} else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
mag = ClimberConstants.MIN_ARM_LENGTH;
} else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
targetPoint.y = 0.d;
mag = ClimberConstants.MIN_ARM_LENGTH;
}
// The angle to the target point
double theta;
if(targetPoint.x == 0.d) {
theta = Math.PI/2.d; // TODO rename variable
} else {
theta = Math.atan(targetPoint.y / targetPoint.x);
}
theta += tiltAngle;
if(targetPoint.x < 0.d)
theta += Math.PI;
// Correct target position for tilt
targetPoint.x = Math.cos(theta) * mag;
targetPoint.y = Math.sin(theta) * mag;
// Law and Order: Cosines edition
double shoulderAngle;
if(mag == 0) {
shoulderAngle = 0;
} else {
shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
(2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
}
shoulderAngle = theta - shoulderAngle;
double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
(2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
//elbowAngle = Math.PI - elbowAngle;
// System.out.println("sa1: " + shoulderAngle);
// System.out.println("ea1: " + elbowAngle);
// If the climber is resting on the robot base, rotate the upper arm in the direction of the target
if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
// System.out.println("xDiff: " + xDiff);
elbowAngle = Math.atan(-targetPoint.y / xDiff);
// System.out.println("ea2: " + elbowAngle);
if(elbowAngle < 0) {
elbowAngle = Math.PI - Math.abs(elbowAngle);
}
if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
// System.out.println("ea3: " + elbowAngle);
// Deal with negative wraparound
// if(xDiff >= 0.d)
// elbowAngle += Math.PI;
// System.out.println("ea4: " + elbowAngle);
}
angles[0] = shoulderAngle;
angles[1] = elbowAngle;
return angles;
}
public static Point getClimberPosition(double shoulderAngle, double elbowAngle) {
Point climberPoint = new Point(0, 0);
climberPoint.x += Math.sin(shoulderAngle);
climberPoint.y += Math.cos(shoulderAngle);
climberPoint.x -= Math.sin(elbowAngle - shoulderAngle);
climberPoint.y += Math.cos(elbowAngle - shoulderAngle);
return climberPoint;
}
public static Point getClimberPosition(double[] jointAngles) {
return getClimberPosition(jointAngles[0], jointAngles[1]);
}
}
src/main/java/frc4388/utility/Vector2D.java
+1 -2
View File
@@ -155,5 +155,4 @@ public class Vector2D extends Vector2d {
public String toString() { public String toString() {
return "<" + this.x + ", " + this.y + ">"; return "<" + this.x + ", " + this.y + ">";
} }
}
}