mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Ryan Manley
@@ -33,6 +33,8 @@ import frc4388.utility.LEDPatterns;
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final double TICKS_PER_ROTATION_FX = 2048;
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 4.0;
|
||||
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 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
|
||||
*/
|
||||
|
||||
@@ -163,6 +163,11 @@ public class Robot extends TimedRobot {
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
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
|
||||
addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)),
|
||||
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")
|
||||
.resolve("recording." + System.currentTimeMillis() + ".path").toFile();
|
||||
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());
|
||||
} else
|
||||
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
|
||||
@@ -247,6 +252,7 @@ public class Robot extends TimedRobot {
|
||||
public void teleopInit() {
|
||||
LOGGER.fine("teleopInit()");
|
||||
|
||||
|
||||
Robot.alliance = DriverStation.getAlliance();
|
||||
|
||||
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);
|
||||
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
|
||||
@@ -59,6 +59,14 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
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.ShooterConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
@@ -82,6 +90,7 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.ListeningSendableChooser;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
@@ -103,6 +112,11 @@ public class RobotContainer {
|
||||
// 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
|
||||
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);
|
||||
@@ -118,7 +132,7 @@ public class RobotContainer {
|
||||
|
||||
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
|
||||
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
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
@@ -127,7 +141,7 @@ public class RobotContainer {
|
||||
|
||||
// Autonomous
|
||||
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 NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
|
||||
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
|
||||
@@ -149,6 +163,17 @@ public class RobotContainer {
|
||||
configureButtonBindings();
|
||||
/* 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
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
@@ -208,6 +233,7 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
|
||||
/* Driver Buttons */
|
||||
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
@@ -215,74 +241,29 @@ public class RobotContainer {
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
// Left Bumper > Shift Down
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
// // Right Bumper > Shift Up
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
.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 */
|
||||
|
||||
// X > Extend Intake
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .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)
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
|
||||
@@ -295,31 +276,11 @@ public class RobotContainer {
|
||||
//Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.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)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));\
|
||||
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), 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
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
// .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_robotExtender.setEncoder(0), 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)
|
||||
// .whileHeld(new TurretManual(m_robotTurret));
|
||||
@@ -372,9 +329,6 @@ public class RobotContainer {
|
||||
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
|
||||
// .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)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
||||
@@ -391,26 +345,27 @@ public class RobotContainer {
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
if (loadedPathTrajectory != null) {
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
// if (loadedPathTrajectory != null) {
|
||||
// PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
// PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
// ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
PathPlannerState initialState = loadedPathTrajectory.getInitialState();
|
||||
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
|
||||
new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
|
||||
m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
|
||||
} else {
|
||||
LOGGER.severe("No auto selected.");
|
||||
return new RunCommand(() -> {
|
||||
}).withName("No Autonomous Path");
|
||||
}
|
||||
// PathPlannerState initialState = loadedPathTrajectory.getInitialState();
|
||||
// Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
// return new SequentialCommandGroup(
|
||||
// new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
|
||||
// new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
|
||||
// m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
|
||||
// m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
|
||||
// new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
|
||||
// } else {
|
||||
// LOGGER.severe("No auto selected.");
|
||||
// return new RunCommand(() -> {
|
||||
// }).withName("No Autonomous Path");
|
||||
// }
|
||||
return null;
|
||||
}
|
||||
|
||||
public XboxController getDriverController() {
|
||||
@@ -434,18 +389,18 @@ public class RobotContainer {
|
||||
*
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
// public Pose2d getOdometry() {
|
||||
// return m_robotSwerveDrive.getOdometry();
|
||||
// }
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
*
|
||||
* @param pose Pose to set odometry to.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
// public void resetOdometry(Pose2d pose) {
|
||||
// m_robotSwerveDrive.resetOdometry(pose);
|
||||
// }
|
||||
|
||||
/**
|
||||
* Creates a WatchKey for the path planner directory and registers it with the
|
||||
@@ -454,31 +409,32 @@ public class RobotContainer {
|
||||
* latest path files.
|
||||
* Finally, adds the existing path files to the auto chooser
|
||||
*/
|
||||
private void autoInit() {
|
||||
try {
|
||||
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
|
||||
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
|
||||
StandardWatchEventKinds.ENTRY_DELETE);
|
||||
// TODO: Store this and other commands as fields so they can be rescheduled.
|
||||
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}.withName("Path Watcher").schedule();
|
||||
} catch (IOException exception) {
|
||||
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
|
||||
}
|
||||
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
|
||||
.filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
|
||||
.forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
}
|
||||
// private void autoInit() {
|
||||
// try {
|
||||
// WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
|
||||
// StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
|
||||
// StandardWatchEventKinds.ENTRY_DELETE);
|
||||
// // TODO: Store this and other commands as fields so they can be rescheduled.
|
||||
// new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
|
||||
// @Override
|
||||
// public boolean runsWhenDisabled() {
|
||||
// return true;
|
||||
// }
|
||||
// }.withName("Path Watcher").schedule();
|
||||
// } catch (IOException exception) {
|
||||
// LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
|
||||
// }
|
||||
// Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
|
||||
// .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
|
||||
// .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
|
||||
// SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
// }
|
||||
|
||||
/**
|
||||
* Creates a button on the SmartDashboard that will record the path of the
|
||||
* robot.
|
||||
*/
|
||||
|
||||
public void recordInit() {
|
||||
SmartDashboard.putData("Recording",
|
||||
new RunCommand(this::recordPeriodic) {
|
||||
|
||||
@@ -4,6 +4,12 @@
|
||||
|
||||
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.RemoteSensorSource;
|
||||
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.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import edu.wpi.first.networktables.NetworkTableType;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
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.LEDConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
@@ -49,7 +57,6 @@ public class RobotMap {
|
||||
// void configureLEDMotorControllers() {}
|
||||
|
||||
/* Swerve Subsystem */
|
||||
|
||||
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 rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
@@ -188,6 +195,16 @@ public class RobotMap {
|
||||
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
|
||||
/* Boom Boom Subsystem */
|
||||
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.setInverted(true);
|
||||
}
|
||||
|
||||
|
||||
/* Serializer Subsystem */
|
||||
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
||||
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 CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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]);
|
||||
}
|
||||
}
|
||||
@@ -155,5 +155,4 @@ public class Vector2D extends Vector2d {
|
||||
public String toString() {
|
||||
return "<" + this.x + ", " + this.y + ">";
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user