diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 37137a4..7930670 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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 */ diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1d2b91f..759e2c9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -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 diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ed4d006..791fcbc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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 autoChooser = new ListeningSendableChooser<>(this::loadPath); + // private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List 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) { diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 065ec83..9638c45 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -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); diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java new file mode 100644 index 0000000..0c4e8b7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -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 + } +} diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java new file mode 100644 index 0000000..38ad651 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java new file mode 100644 index 0000000..2d8f726 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -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(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java new file mode 100644 index 0000000..ef6e973 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -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 + *

+ * 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]); + } +} diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 9ee4fc5..5a9bfcd 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -155,5 +155,4 @@ public class Vector2D extends Vector2d { public String toString() { return "<" + this.x + ", " + this.y + ">"; } - -} \ No newline at end of file +}