-
Notifications
You must be signed in to change notification settings - Fork 0
/
wsce_display_bot.ino
115 lines (91 loc) · 2.21 KB
/
wsce_display_bot.ino
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
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include "intake.hpp"
#include "pitcher.hpp"
#include "shooter.hpp"
#include "robot_defs.hpp"
typedef enum
{
ACTUATE_INTAKE = 0,
ACTUATE_SHOOTER,
ACTUATE_PITCHER
} ActuationState;
Servo shooter_flywheel;
Servo intake_motor;
Servo pitcher_motor;
Intake *intake; //{intake_motor};
Shooter *shooter; //{shooter_flywheel};
Pitcher *pitcher; //{pitcher_motor};
unsigned long last_state_transition_time;
unsigned long last_tick_time;
ActuationState state;
void intake_callback()
{
shooter->stop();
pitcher->lower();
intake->spin();
}
void shooter_callback()
{
pitcher->lower();
intake->stop();
shooter->shoot();
}
void pitcher_callback()
{
intake->stop();
shooter->stop();
pitcher->raise();
}
void setup()
{
// put your setup code here, to run once
last_state_transition_time = 0U;
state = ACTUATE_INTAKE;
last_tick_time = 0U;
intake = new Intake(intake_motor);
shooter = new Shooter(shooter_flywheel);
pitcher = new Pitcher(pitcher_motor);
}
void loop()
{
// put your main code here, to run repeatedly:
if ((millis() - last_tick_time) > 200)
{
if ((millis() - last_state_transition_time) > 5000U)
{
switch (state)
{
case ACTUATE_INTAKE:
state = ACTUATE_PITCHER;
break;
case ACTUATE_PITCHER:
state = ACTUATE_SHOOTER;
break;
case ACTUATE_SHOOTER:
state = ACTUATE_INTAKE;
break;
default:
break;
}
last_state_transition_time = millis();
}
switch (state)
{
case ACTUATE_INTAKE:
intake_callback();
break;
case ACTUATE_PITCHER:
pitcher_callback();
break;
case ACTUATE_SHOOTER:
shooter_callback();
break;
default:
break;
}
// Tick all the things
intake->tick();
shooter->tick();
pitcher->tick();
last_tick_time = millis();
}
}