src/main/java/frc/robot/util/MechState.java


1
2
3
4
5
6
7
8
9
10
11
12
13
14
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;
     }
}