1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
|
package frc.robot.subsystems;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.commands.IntakeIntermediate;
import frc.robot.util.MechState;
public class MainMech extends SubsystemBase
{
public final Arm m_arm = new Arm();
public final Chain m_chain = new Chain();
public enum MechCmd {
INTAKE,
SPEAKER,
AMP,
DEFAULT,
MANUAL
}
private double deadzone(double low, double high, double val)
{
if(val > low && val < high) return 0.0;
return ((val > 0.0) ? val - high : val - low);
}
public MechCmd current_state = MechCmd.SPEAKER;
public Command m_manual_cmd = this.run(() -> {
});
public MechState states[] = {
new MechState(0.8, -48),
new MechState(0, -42),
new MechState(0.95, 54.5),
new MechState(0.5, -20),
new MechState(0.5, 20)
};
public MainMech() { }
public void setDefaultCommands(Command arm_command, Command chain_command)
{
m_arm.setDefaultCommand(arm_command);
m_chain.setDefaultCommand(chain_command);
}
public Command command(MechState state) {
Command c = Commands.parallel(
m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)),
m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
c.addRequirements(this);
return c;
}
public Command toState(MechCmd state)
{
MechState next = states[state.ordinal()];
if (next.isValid()) {
Command next_cmd = command(next);
return next_cmd;
}
return Commands.none();
}
@Override
public void periodic() { }
}
|
