blob: 80bcd3193f1665a5aee0186b88689622694fa46a [file] [log] [blame]
Austin Schuh4fae0fc2018-03-27 23:51:42 -07001#include "motors/seems_reasonable/spring.h"
2
Austin Schuh4fae0fc2018-03-27 23:51:42 -07003#include <cmath>
4
Philipp Schrader790cb542023-07-05 21:06:52 -07005#include "frc971/zeroing/wrap.h"
6
Stephan Pleinesf63bde82024-01-13 15:59:33 -08007namespace motors::seems_reasonable {
Austin Schuh4fae0fc2018-03-27 23:51:42 -07008namespace {
9
10constexpr float kTwoPi = 2.0 * M_PI;
11
12} // namespace
13
14float NextGoal(float current_goal, float goal) {
15 float remainder = remainderf(current_goal - goal, kTwoPi);
16 if (remainder >= 0.0f) {
17 remainder -= kTwoPi;
18 }
19 return -remainder + current_goal;
20}
21
22float PreviousGoal(float current_goal, float goal) {
23 float remainder = remainderf(current_goal - goal, kTwoPi);
24 if (remainder <= 0.0f) {
25 remainder += kTwoPi;
26 }
27 return -remainder + current_goal;
28}
29
30void Spring::Iterate(bool unload, bool prime, bool fire, bool force_reset,
Austin Schuhe666dc62018-08-08 21:09:12 -070031 bool force_move, bool encoder_valid, float angle) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -070032 // Angle is +- M_PI. So, we need to find the nearest angle to the previous
33 // one, and that's our new angle.
34 angle_ = ::frc971::zeroing::Wrap(angle_, angle, kTwoPi);
35
36 switch (state_) {
37 case State::UNINITIALIZED:
38 // Go to the previous unload from where we are.
39 goal_ = angle_;
40 goal_ = PreviousGoal(kUnloadGoal);
Austin Schuhe666dc62018-08-08 21:09:12 -070041 if (force_move) {
42 ForceMove();
43 } else if (prime && fire) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -070044 Unload();
45 }
46 break;
Austin Schuhe666dc62018-08-08 21:09:12 -070047 case State::FORCE_MOVE:
48 if (!force_move) {
49 state_ = State::UNINITIALIZED;
50 }
51 break;
Austin Schuh4fae0fc2018-03-27 23:51:42 -070052 case State::UNLOAD:
Austin Schuhe666dc62018-08-08 21:09:12 -070053 if (force_move) {
54 ForceMove();
55 } else if (!encoder_valid) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -070056 state_ = State::STUCK_UNLOAD;
57 } else if (!unload && prime && fire) {
58 // Go to the next goal from the current location. This handles if we
59 // fired or didn't on the previous cycle.
60 goal_ = angle_;
61 goal_ = NextGoal(kLoadGoal);
62 Load();
63 }
Brian Silverman1eea1132018-09-02 16:29:53 -070064 // TODO(Austin): This should be a break, right?
65 // fallthrough
Austin Schuh4fae0fc2018-03-27 23:51:42 -070066 case State::STUCK_UNLOAD:
Austin Schuhe666dc62018-08-08 21:09:12 -070067 if (force_move) {
68 ForceMove();
69 } else if (force_reset && encoder_valid &&
70 state_ == State::STUCK_UNLOAD) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -070071 state_ = State::UNINITIALIZED;
72 } else if (timeout_ > 0) {
73 --timeout_;
74 }
75 break;
76 case State::LOAD:
Austin Schuhe666dc62018-08-08 21:09:12 -070077 if (force_move) {
78 ForceMove();
79 } else if (!encoder_valid) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -070080 goal_ = PreviousGoal(kUnloadGoal);
81 StuckUnload();
82 } else if (unload) {
83 goal_ = PreviousGoal(kUnloadGoal);
84 Unload();
85 } else if (!Near()) {
86 if (timeout_ > 0) {
87 --timeout_;
88 } else {
89 StuckUnload();
90 }
91 } else if (prime) {
92 goal_ = NextGoal(kPrimeGoal);
93 Prime();
94 }
95 break;
96 case State::PRIME:
Austin Schuhe666dc62018-08-08 21:09:12 -070097 if (force_move) {
98 ForceMove();
99 } else if (!encoder_valid) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700100 goal_ = PreviousGoal(kUnloadGoal);
101 StuckUnload();
102 } else if (unload) {
103 goal_ = PreviousGoal(kUnloadGoal);
104 Unload();
105 } else if (!prime) {
106 goal_ = PreviousGoal(kLoadGoal);
107 Load();
108 } else if (!Near()) {
109 if (timeout_ > 0) {
110 --timeout_;
111 } else {
112 StuckUnload();
113 }
114 } else if (fire) {
115 goal_ = NextGoal(kFireGoal);
116 Fire();
117 }
118 break;
119
120 case State::FIRE:
Austin Schuhe666dc62018-08-08 21:09:12 -0700121 if (force_move) {
122 ForceMove();
123 } else if (!encoder_valid) {
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700124 goal_ = PreviousGoal(kUnloadGoal);
125 StuckUnload();
126 } else if (!Near()) {
127 if (timeout_ > 0) {
128 --timeout_;
129 } else {
130 StuckUnload();
131 }
132 } else {
133 // TODO(austin): Maybe have a different timeout for success.
134 if (timeout_ > 0) {
135 timeout_--;
Austin Schuh72656c42018-04-01 16:37:52 -0700136 } else if (!prime) {
137 state_ = State::WAIT_FOR_LOAD;
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700138 }
139 }
140 break;
Austin Schuh72656c42018-04-01 16:37:52 -0700141 case State::WAIT_FOR_LOAD:
Austin Schuhe666dc62018-08-08 21:09:12 -0700142 if (force_move) {
143 ForceMove();
144 } else if (!encoder_valid) {
Austin Schuh72656c42018-04-01 16:37:52 -0700145 StuckUnload();
146 } else if (unload) {
147 // Goal is as good as it is going to get since unload is the fire
148 // position.
149 Unload();
150 } else if (prime) {
151 state_ = State::WAIT_FOR_LOAD_RELEASE;
152 }
153 break;
154 case State::WAIT_FOR_LOAD_RELEASE:
Austin Schuhe666dc62018-08-08 21:09:12 -0700155 if (force_move) {
156 ForceMove();
157 } else if (!encoder_valid) {
Austin Schuh72656c42018-04-01 16:37:52 -0700158 StuckUnload();
159 } else if (unload) {
160 // Goal is as good as it is going to get since unload is the fire
161 // position.
162 Unload();
163 } else if (!prime) {
164 Load();
165 goal_ = NextGoal(kLoadGoal);
166 }
167 break;
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700168 }
169 const float error = goal_ - angle_;
170 const float derror = (error - last_error_) * 200.0f;
171
172 switch (state_) {
173 case State::UNINITIALIZED:
174 output_ = 0.0f;
175 break;
176 case State::STUCK_UNLOAD:
177 case State::UNLOAD:
178 if (timeout_ > 0) {
179 output_ = -0.1f;
180 } else {
181 output_ = 0.0f;
182 }
183 break;
184
Austin Schuhe666dc62018-08-08 21:09:12 -0700185 case State::FORCE_MOVE:
186 output_ = 1.0f;
187 break;
188
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700189 case State::LOAD:
190 case State::PRIME:
Austin Schuh72656c42018-04-01 16:37:52 -0700191 case State::FIRE:
192 case State::WAIT_FOR_LOAD:
193 case State::WAIT_FOR_LOAD_RELEASE: {
Austin Schuh4fae0fc2018-03-27 23:51:42 -0700194 constexpr float kP = 3.00f;
195 constexpr float kD = 0.00f;
196 output_ = kP * error + kD * derror;
197 } break;
198 }
199 last_error_ = error;
200}
201
Stephan Pleinesf63bde82024-01-13 15:59:33 -0800202} // namespace motors::seems_reasonable