From 89d80968447e8a59ca5ab1527697d8cde5434d40 Mon Sep 17 00:00:00 2001
From: Abhi Sachi <abhi.p.sachi@gmail.com>
Date: Sat, 21 Jan 2023 13:22:37 -0700
Subject: [PATCH] defualt command and soft limits

---
 src/main/java/frc4388/robot/Constants.java      | 10 ++++++++++
 src/main/java/frc4388/robot/RobotContainer.java |  8 ++++++++
 src/main/java/frc4388/robot/RobotMap.java       | 17 +++++++++++++++++
 src/main/java/frc4388/robot/subsystems/Arm.java | 15 ++++++++++++++-
 4 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 9b39fd4..148dea2 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -104,6 +104,16 @@ public final class Constants {
   public static final class GyroConstants {
     public static final int ID = -1; // TODO: find the actual ID
   }
+
+  public static final class ArmConstants {
+    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 class LEDConstants {
     public static final int LED_SPARK_ID = 0;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a42a463..edbac4f 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
@@ -33,7 +34,9 @@ public class RobotContainer {
 
     /* Subsystems */
     private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro);
+    private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele);
     private final LED m_robotLED = new LED(m_robotMap.LEDController);
+
     
 
     /* Controllers */
@@ -58,6 +61,11 @@ public class RobotContainer {
                                                                     getDriverController().getLeftYAxis(), 
                                                                    -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)
         );
+
+        m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.runPivotAndTele(
+            getOperatorController().getLeftYAxis(), 
+            getOperatorController().getLeftXAxis()), m_robotArm)
+        );
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 041cc7e..41a2f83 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -7,11 +7,13 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
@@ -145,5 +147,20 @@ public class RobotMap {
     // config closed loop ramp
     pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
     tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    // config neutral mode to brake
+    pivot.setNeutralMode(NeutralMode.Brake);
+    tele.setNeutralMode(NeutralMode.Brake);
+
+    // soft limits
+    pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
+    pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
+    pivot.configForwardSoftLimitEnable(false);
+    pivot.configReverseSoftLimitEnable(false);
+
+    tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+    tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+    tele.configForwardSoftLimitEnable(false);
+    tele.configReverseSoftLimitEnable(false);
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 85e8860..af214ba 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -22,10 +22,23 @@ public class Arm extends SubsystemBase {
     pivot.set(output);
   }
 
-  public void runTelescope(double 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() {
     // This method will be called once per scheduler run