From 4ef390a6f44f001de5adbdbbc5c8ca6993e3ab61 Mon Sep 17 00:00:00 2001
From: Unknown <hoxiealling@gmail.com>
Date: Thu, 31 Jan 2019 12:39:47 -0500
Subject: [PATCH] Added functionality to Wrist Subsytem

Issue #44
---
 .../robot/commands/Wrist/ResetWristEncoder.kt | 10 +++
 .../robot/commands/Wrist/ToggleIntakeMode.kt  |  0
 src/main/java/frc/robot/subsystems/Wrist.kt   | 68 ++++++++++++++-----
 3 files changed, 60 insertions(+), 18 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/Wrist/ResetWristEncoder.kt
 create mode 100644 src/main/java/frc/robot/commands/Wrist/ToggleIntakeMode.kt

diff --git a/src/main/java/frc/robot/commands/Wrist/ResetWristEncoder.kt b/src/main/java/frc/robot/commands/Wrist/ResetWristEncoder.kt
new file mode 100644
index 0000000..345fd65
--- /dev/null
+++ b/src/main/java/frc/robot/commands/Wrist/ResetWristEncoder.kt
@@ -0,0 +1,10 @@
+package frc.robot.commands.Wrist
+
+import org.sertain.command.Command
+import frc.robot.subsystems.Wrist
+import frc.robot.OI
+
+public class ResetWristEncoders(position: String): Command() //done after auto unfold is complete
+{
+    Wrist.ResetEncoders()
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/Wrist/ToggleIntakeMode.kt b/src/main/java/frc/robot/commands/Wrist/ToggleIntakeMode.kt
new file mode 100644
index 0000000..e69de29
diff --git a/src/main/java/frc/robot/subsystems/Wrist.kt b/src/main/java/frc/robot/subsystems/Wrist.kt
index 5fa1895..8ce7b0c 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.kt
+++ b/src/main/java/frc/robot/subsystems/Wrist.kt
@@ -20,9 +20,12 @@ public object Wrist : Subsystem()
     //configuring motion magic
     var cruiseVelocity: Double = 19000.0  //temp
     var acceleration: Double = 11000.0  //temp
-    var topHeight: Double = 72000.0 //temp
-    var middleHeight: Double = 35000.0//temp
-    var bottomHeight: Double = 0.0//temp
+    var topHeightPanel: Double = 72000.0 //temp
+    var middleHeightPanel: Double = 35000.0//temp
+    var bottomHeightPanel: Double = 0.0//temp
+    var topHeightCargo: Double = 72000.0 //temp
+    var middleHeightCargo: Double = 35000.0//temp
+    var bottomHeightCargo: Double = 0.0//temp
 
     //configuring PID Loop for motion magic to do- move to IDS
     var kPIDLoopIdx: Int = 0
@@ -35,7 +38,7 @@ public object Wrist : Subsystem()
     var kGainskD: Double = 0.0 
 
     var wristState: String = "Bottom"
-
+    var intakeType: Boolean = false //false is ball true is panel
     override fun onCreate()
     {
         wristMotor.configFactoryDefault()
@@ -85,26 +88,55 @@ public object Wrist : Subsystem()
     fun wristMotionMagic (newWristState: String = "Null")
     {
         var targetPos = newWristState
-        if(targetPos=="Bottom" && targetPos != wristState )
+        if (intakeType) //panel
         {
-            wristMotor.set(ControlMode.MotionMagic, bottomHeight)
-            wristMotor.set(ControlMode.MotionMagic, bottomHeight)
-            wristState="Bottom"
+            if(targetPos=="Bottom" && targetPos != wristState )
+            {
+                wristMotor.set(ControlMode.MotionMagic, bottomHeightPanel)
+                wristMotor.set(ControlMode.MotionMagic, bottomHeightPanel)
+                wristState="Bottom"
+            }
+            if(targetPos=="Middle" && targetPos !=wristState)
+            {
+                wristMotor.set(ControlMode.MotionMagic, middleHeightPanel)
+                wristMotor.set(ControlMode.MotionMagic, middleHeightPanel)
+                wristState="Middle"
+            }
+            if(targetPos=="Top" && targetPos != wristState)
+            {
+                wristMotor.set(ControlMode.MotionMagic, topHeightPanel)
+                wristMotor.set(ControlMode.MotionMagic, topHeightPanel)
+                wristState="Bottom"
+            }     
         }
-        if(targetPos=="Middle" && targetPos !=wristState)
+        else  //cargo
         {
-            wristMotor.set(ControlMode.MotionMagic, middleHeight)
-            wristMotor.set(ControlMode.MotionMagic, middleHeight)
-            wristState="Middle"
+            if(targetPos=="Bottom" && targetPos != wristState )
+            {
+                wristMotor.set(ControlMode.MotionMagic, bottomHeightCargo)
+                wristMotor.set(ControlMode.MotionMagic, bottomHeightCargo)
+                wristState="Bottom"
+            }
+            if(targetPos=="Middle" && targetPos !=wristState)
+            {
+                wristMotor.set(ControlMode.MotionMagic, middleHeightCargo)
+                wristMotor.set(ControlMode.MotionMagic, middleHeightCargo)
+                wristState="Middle"
+            }
+            if(targetPos=="Top" && targetPos != wristState)
+            {
+                wristMotor.set(ControlMode.MotionMagic, topHeightCargo)
+                wristMotor.set(ControlMode.MotionMagic, topHeightCargo)
+                wristState="Bottom"
+            }    
         }
-        if(targetPos=="Top" && targetPos != wristState)
-        {
-            wristMotor.set(ControlMode.MotionMagic, topHeight)
-            wristMotor.set(ControlMode.MotionMagic, topHeight)
-            wristState="Bottom"
-        }     
     }
 
+    fun toggleIntakeType() : Boolean
+    {
+        intakeType = !intakeType
+        return intakeType
+    }
     // Returning the state the arm is in 
     fun returnWristState(): String { return wristState; }