From b891f5e809f9b222a2d6d760c5d4a9510dff3651 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Thu, 9 Feb 2023 19:13:55 -0700 Subject: [PATCH] arm limits --- src/main/java/frc4388/robot/Constants.java | 11 +- .../java/frc4388/robot/RobotContainer.java | 5 +- src/main/java/frc4388/robot/RobotMap.java | 1 - .../java/frc4388/robot/subsystems/Arm.java | 120 ++++++++++-------- 4 files changed, 77 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 148dea2..cc3493d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -106,13 +106,16 @@ public final class Constants { } public static final class ArmConstants { + public static final double MIN_ARM_LEN = 1; + public static final double MAX_ARM_LEN = 2; + public static final double ARM_HEIGHT = 1; + public static final double CURVE_POWER = 2; + public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value - public static final double TELE_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value - public static final double TELE_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value - - + public static final double TELE_FORWARD_SOFT_LIMIT = 0; + public static final double TELE_REVERSE_SOFT_LIMIT = 0; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index edbac4f..2f994a9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,9 +62,8 @@ public class RobotContainer { -getDriverController().getRightXAxis(), false), m_robotSwerveDrive) ); - m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.runPivotAndTele( - getOperatorController().getLeftYAxis(), - getOperatorController().getLeftXAxis()), m_robotArm) + m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength( + getOperatorController().getLeftYAxis()), m_robotArm) ); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 41a2f83..faf7ce2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -63,7 +63,6 @@ public class RobotMap { public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - void configureDriveMotors() { // config factory default leftFrontWheel.configFactoryDefault(); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 87feb56..7b71a03 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,60 +1,76 @@ -// 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 com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - +import frc4388.robot.Constants.ArmConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; public class Arm extends SubsystemBase { - /** Creates a new Arm. */ - private WPI_TalonFX pivot; - private WPI_TalonFX tele; - - - - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { - this.pivot = pivot; - this.tele = tele; - - //Limit switches - pivot.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - pivot.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - - tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - tele.configForwardSoftLimitThreshold(Constants.ArmConstants.TELE_FORWARD_SOFT_LIMIT); - } - - public void runPivot(double output) { - pivot.set(output); - } - - public void runTele(double output) { - tele.set(output); - } - - public void runPivotAndTele(double pOutput, double tOutput) { - pivot.set(pOutput); - tele.set(tOutput); - } - - public double getPivotPos() { - return pivot.getSelectedSensorPosition(); - } - - public double getTelePos() { - return tele.getSelectedSensorPosition(); - } - - @Override - public void periodic() { - if (tele.isRevLimitSwitchClosed() == 1) { - tele.setSelectedSensorPosition(0); + private WPI_TalonFX m_tele; + private WPI_TalonFX m_pivot; + private boolean m_debug; + // Moves arm to distence [dist] then returns new ang + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, boolean debug) { + tele.configFactoryDefault(); + m_tele = tele; + pivot.configFactoryDefault(); + m_pivot = pivot; + } + + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { + this(pivot, tele, false); + } + + public void armSetRotation(double rot) { + if (rot > 1 || rot < 0) return; + // Move arm code + m_pivot.set(ControlMode.Position, rot * (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + + ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + } + + public void armSetLength(double len) { + if (len > 1 || len < 0) return; + // Move arm code + m_tele.set(ControlMode.Position, len * (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + + ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } + + public double getArmLength() { + return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / + (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } + + public void runPivotTele(double pivot, double tele) { + var rot = 0; + + if (checkLimits(tele, rot)) { + armSetRotation(pivot); + armSetLength(tele); + } + } + + /** + * Checks that an input is within bounds + * @param _len length from 0 to 1 + * @param _theta radians from the zero (straight up) + * @return + */ + public static boolean checkLimits(double _len, double _theta) { + var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN; + var x = len * Math.cos(_theta); + var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); + + var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); + if (y < minHeight) + return false; + + return true; + } + + @Override + public void periodic() { + if (m_debug) + SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); } - } }