changes

1 files changed, 15 insertions, 0 deletions

diff --git a/src/main/java/frc/robot/util/MechState.java b/src/main/java/frc/robot/util/MechState.java
new file mode 100644
index 0000000..f0092f0
--- /dev/null
+++ b/src/main/java/frc/robot/util/MechState.java
@@ -0,0 +1,15 @@
+package frc.robot.util;
+
+public class MechState {
+    public double m_chain_pos;
+    public double m_arm_pos;
+    public MechState(double chain_pos, double arm_pos) {
+        m_chain_pos = chain_pos;
+        m_arm_pos = arm_pos;
+    }
+    // TODO
+    public boolean isValid() { 
+        return 30 > (Math.cos(m_arm_pos * Math.PI / 180) * 60 - m_arm_pos * 80);
+        // return true;
+     }
+}