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
72
73
74
75
76
77
78
79
80
81
82
83
84
|
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.95, -40),
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) {
return Commands.parallel(
m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)),
m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
}
public Command toState(MechCmd state)
{
MechState next = states[state.ordinal()];
if (next.isValid()) {
Command next_cmd = command(next);
if(state == MechCmd.INTAKE || current_state == MechCmd.INTAKE) {
current_state = state;
return new IntakeIntermediate(m_arm, m_chain).andThen(next_cmd);
}
if (state == MechCmd.MANUAL) {
current_state = state;
double arm_pos = m_arm.pos() + deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2));
double chain_pos = Constants.controller2.getRawAxis(0) * (-0.5) + 0.5;
MechState manual_state = new MechState(chain_pos, arm_pos);
if (manual_state.isValid()) {
return command(manual_state);
}
return Commands.none();
}
current_state = state;
return next_cmd;
}
return Commands.none();
}
@Override
public void periodic() { }
}
|
