From ef6613a7cdeb874a6aa1bc4fd64d7fa3bf7f9d52 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <abhi.p.sachi@gmail.com>
Date: Fri, 17 Mar 2023 17:00:34 -0600
Subject: [PATCH] tele no PID, cube high/mid needs further testing

---
 .../java/frc4388/robot/RobotContainer.java    | 22 ++++++++++-------
 .../robot/commands/Arm/TeleCommand.java       | 24 ++++++++++++-------
 .../java/frc4388/robot/subsystems/Arm.java    | 10 ++++++--
 3 files changed, 36 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index aaed46a..f6d8239 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -114,16 +114,19 @@ public class RobotContainer {
     };
 
     // TODO: find actual values
-    private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
+    private SequentialCommandGroup placeConeHigh =
+        new SequentialCommandGroup(
+            // new InstantCommand(() -> System.out.println("Placing cone high")),
+            new PivotCommand(m_robotArm, 64 + 135),
+            new InstantCommand(() -> m_robotArm.setRotVel(0)),
+            new TeleCommand(m_robotArm, 95642),
+            toggleClaw.asProxy(),
+            armToHome.asProxy()
+        );
 
     private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
+        new PivotCommand(m_robotArm, 70 + 135),
+        new TeleCommand(m_robotArm, 32866),
         toggleClaw.asProxy(),
         armToHome.asProxy()
     );
@@ -247,7 +250,8 @@ public class RobotContainer {
         
         // interrupt button
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(interruptCommand.asProxy());
+            .onTrue(placeConeHigh.asProxy());
+            // .onTrue(interruptCommand.asProxy());
 
         // place high
         new POVButton(getDeadbandedOperatorController(), 0)
diff --git a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
index 356b2d6..a5a6e9f 100644
--- a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
+++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
@@ -4,30 +4,36 @@
 
 package frc4388.robot.commands.Arm;
 
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.commands.PelvicInflammatoryDisease;
 import frc4388.robot.subsystems.Arm;
 
-public class TeleCommand extends PelvicInflammatoryDisease {
-  private final Arm    arm;
-  private final double target;
+public class TeleCommand extends CommandBase {
+  private final Arm     arm;
+  private final double  target;
+  private       boolean goIn;
   
   /** Creates a new ArmCommand. */
   public TeleCommand(Arm arm, double target) {
-    super(0.6, 0, 0, 0, 0);
     this.arm    = arm;
     this.target = target;
     addRequirements(arm);
   }
 
   @Override
-  public double getError() {
-    return (arm.getArmLength() - target) /
-           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+  public void initialize() {
+    this.goIn = target < arm.getArmLength();
   }
 
   @Override
-  public void runWithOutput(double output) {
-    arm.setTeleVel(output);
+  public void execute() {
+    arm.setTeleVel(goIn ? 1 : -1);
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (goIn) return arm.getArmLength() < target;
+    else      return arm.getArmLength() > target;
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 47beeb2..94081df 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -40,6 +40,8 @@ public class Arm extends SubsystemBase {
     }
 
     public void setRotVel(double vel) {
+        if (vel > 1) vel = 1;
+
         var degrees = Math.abs(getArmRotation()) - 135;
         SmartDashboard.putNumber("arm degrees", degrees);
         SmartDashboard.putNumber("arm rot vel", vel);
@@ -48,6 +50,8 @@ public class Arm extends SubsystemBase {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
             m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+        } else if (degrees < 25 && vel < 0) {
+            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
         } else {
             m_pivot.set(ControlMode.PercentOutput, .3 * vel);
         }
@@ -78,7 +82,7 @@ public class Arm extends SubsystemBase {
     }
 
     public double getArmLength() {
-        return m_tele.getSelectedSensorPosition();
+        return m_tele.getSelectedSensorPosition() - tele_soft;
     }
 
     public double getArmRotation() {
@@ -112,9 +116,10 @@ public class Arm extends SubsystemBase {
     }
 
     boolean tele_softLimit = false;
+    double  tele_soft = 0;
     public void resetTeleSoftLimit() {
         if (!tele_softLimit) {
-            var tele_soft = m_tele.getSelectedSensorPosition();
+            tele_soft = m_tele.getSelectedSensorPosition();
             m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
             m_tele.configReverseSoftLimitThreshold(tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
@@ -145,6 +150,7 @@ public class Arm extends SubsystemBase {
         }
 
         // double x = Math.cos(Math.toRadians(degrees));
+        SmartDashboard.putNumber("arm length", getArmLength());
     }
 
     public void killSoftLimits() {