arm limits

2026-06-09 00:37:59 -06:00 · 2023-02-09 19:13:55 -07:00
parent 5d37813c8b
commit b891f5e809
4 changed files with 77 additions and 60 deletions
@@ -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());
    }
-  }
 }