arm limits

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_FORWARD_SOFT_LIMIT = 0;
-    public static final double TELE_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value
+    public static final double TELE_REVERSE_SOFT_LIMIT = 0;
-
-
   }
     
   public static final class LEDConstants {

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(
+        m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength(
-            getOperatorController().getLeftYAxis(), 
+            getOperatorController().getLeftYAxis()), m_robotArm)
-            getOperatorController().getLeftXAxis()), m_robotArm)
         );
     }
 

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();

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.ControlMode;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 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 m_tele;
-  private WPI_TalonFX pivot;
+    private WPI_TalonFX m_pivot;
-  private WPI_TalonFX tele;
+    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();
-  public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) {
+        m_tele = tele;
-    this.pivot = pivot;
+        pivot.configFactoryDefault();
-    this.tele = tele;
+        m_pivot = pivot;
-    
+    }
-    //Limit switches
+
-    pivot.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+    public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) {
-    pivot.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+        this(pivot, tele, false);
-
+    }
-    tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+
-    tele.configForwardSoftLimitThreshold(Constants.ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+    public void armSetRotation(double rot) {
-  }
+        if (rot > 1 || rot < 0) return;
-
+        // Move arm code
-  public void runPivot(double output) {
+        m_pivot.set(ControlMode.Position, rot * (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
-    pivot.set(output);
+            ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
-  }
+    }
-
+
-  public void runTele(double output) {
+    public void armSetLength(double len) {
-    tele.set(output);
+        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) +
-  public void runPivotAndTele(double pOutput, double tOutput) {
+            ArmConstants.TELE_FORWARD_SOFT_LIMIT);
-    pivot.set(pOutput);
+    }
-    tele.set(tOutput);
+
-  }
+    public double getArmLength() {
-
+        return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
-  public double getPivotPos() {
+            (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
-    return pivot.getSelectedSensorPosition();
+    }
-  }
+
-
+    public void runPivotTele(double pivot, double tele) {
-  public double getTelePos() {
+        var rot = 0;
-    return tele.getSelectedSensorPosition();
+
-  }
+        if (checkLimits(tele, rot)) {
-
+            armSetRotation(pivot);
-  @Override
+            armSetLength(tele);
-  public void periodic() {
+        }
-    if (tele.isRevLimitSwitchClosed() == 1) {
+    }
-      tele.setSelectedSensorPosition(0);
+
+    /**
+     * 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());
     }
-  }
 }