blob: a1181aeaed5a61c596e09725edb25716ec3d43b2 [file] [log] [blame]
Brian Silverman20141f92015-01-05 17:39:01 -08001#include <stdio.h>
2#include <string.h>
3#include <unistd.h>
4#include <math.h>
5
6#include "aos/linux_code/init.h"
7#include "aos/prime/input/joystick_input.h"
8#include "aos/common/input/driver_station_data.h"
9#include "aos/common/logging/logging.h"
10#include "aos/common/util/log_interval.h"
11#include "aos/common/time.h"
Ben Fredricksond69f38b2015-01-28 20:06:15 -080012#include "aos/common/actions/actions.h"
Brian Silverman20141f92015-01-05 17:39:01 -080013
Daniel Petti61896522015-02-15 18:01:43 -080014#include "frc971/control_loops/claw/claw.q.h"
Brian Silverman20141f92015-01-05 17:39:01 -080015#include "frc971/control_loops/drivetrain/drivetrain.q.h"
Daniel Petti61896522015-02-15 18:01:43 -080016#include "frc971/control_loops/fridge/fridge.q.h"
Brian Silverman20141f92015-01-05 17:39:01 -080017#include "frc971/constants.h"
18#include "frc971/queues/gyro.q.h"
19#include "frc971/autonomous/auto.q.h"
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -080020#include "frc971/actors/fridge_profile_action.q.h"
21#include "frc971/actors/fridge_profile_actor.h"
Brian Silverman20141f92015-01-05 17:39:01 -080022
Daniel Petti61896522015-02-15 18:01:43 -080023using ::frc971::control_loops::claw_queue;
Brian Silvermanada5f2c2015-02-01 02:41:14 -050024using ::frc971::control_loops::drivetrain_queue;
Daniel Petti61896522015-02-15 18:01:43 -080025using ::frc971::control_loops::fridge_queue;
Brian Silverman20141f92015-01-05 17:39:01 -080026using ::frc971::sensors::gyro_reading;
27
28using ::aos::input::driver_station::ButtonLocation;
29using ::aos::input::driver_station::JoystickAxis;
30using ::aos::input::driver_station::ControlBit;
31
32namespace frc971 {
33namespace input {
34namespace joysticks {
35
Brian Silvermanceba6f22015-02-21 15:44:24 -050036/*
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -080037// preset motion limits
38static const double kArmDebugVelocity = 0.17;
39static const double kArmDebugAcceleration = 0.8;
40static const double kElevatorDebugVelocity = 0.2;
41static const double kElevatorDebugAcceleration = 2.2;
Brian Silvermanceba6f22015-02-21 15:44:24 -050042*/
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -080043
Brian Silverman20141f92015-01-05 17:39:01 -080044const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
45const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
46const ButtonLocation kQuickTurn(1, 5);
47
Daniel Petti61896522015-02-15 18:01:43 -080048// TODO(danielp): Real buttons for all of these.
Austin Schuh8a436e82015-02-16 23:31:28 -080049const ButtonLocation kElevatorUp(3, 10);
Austin Schuhcde7ffe2015-02-20 22:10:41 -080050const ButtonLocation kElevatorDown(3, 3);
Austin Schuh8a436e82015-02-16 23:31:28 -080051const ButtonLocation kArmUp(3, 8);
Austin Schuhcde7ffe2015-02-20 22:10:41 -080052const ButtonLocation kArmDown(2, 6);
Austin Schuh8a436e82015-02-16 23:31:28 -080053const ButtonLocation kClawUp(3, 7);
54const ButtonLocation kClawDown(3, 6);
Austin Schuhcde7ffe2015-02-20 22:10:41 -080055const ButtonLocation kClawOpen(3, 11);
56const ButtonLocation kClawClosed(3, 5);
57const ButtonLocation kFridgeOpen(3, 1);
58const ButtonLocation kFridgeClosed(2, 11);
59const ButtonLocation kRollersIn(3, 4);
60const ButtonLocation kClawMiddle(3, 2);
Daniel Petti61896522015-02-15 18:01:43 -080061
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -080062// TODO(ben): Real buttons for all of these.
63const ButtonLocation kArmPresetOne(99, 99);
64const ButtonLocation kArmPresetTwo(99, 99);
65const ButtonLocation kElevatorPresetOne(99, 99);
66const ButtonLocation kElevatorPresetTwo(99, 99);
67
Daniel Petti61896522015-02-15 18:01:43 -080068// Testing mode.
Austin Schuh8a436e82015-02-16 23:31:28 -080069const double kElevatorVelocity = 0.5;
70const double kArmVelocity = 0.5;
71const double kClawVelocity = 2.0;
Daniel Petti61896522015-02-15 18:01:43 -080072// TODO(danielp): Verify.
73const double kJoystickDt = 0.01;
Brian Silverman20141f92015-01-05 17:39:01 -080074
75class Reader : public ::aos::input::JoystickInput {
76 public:
Austin Schuh331e13d2015-02-15 00:16:51 -080077 Reader() : was_running_(false) {}
Brian Silverman20141f92015-01-05 17:39:01 -080078
79 virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
Austin Schuh6182f8d2015-02-14 22:15:04 -080080 bool last_auto_running = auto_running_;
81 auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
Brian Silvermane1103e62015-02-15 02:03:38 -050082 data.GetControlBit(ControlBit::kEnabled);
Austin Schuh6182f8d2015-02-14 22:15:04 -080083 if (auto_running_ != last_auto_running) {
84 if (auto_running_) {
85 StartAuto();
86 } else {
87 StopAuto();
Brian Silverman20141f92015-01-05 17:39:01 -080088 }
Austin Schuh6182f8d2015-02-14 22:15:04 -080089 }
90
91 if (!data.GetControlBit(ControlBit::kAutonomous)) {
Daniel Petti61896522015-02-15 18:01:43 -080092 HandleDrivetrain(data);
93 if (data.GetControlBit(ControlBit::kTestMode)) {
94 HandleTest(data);
95 } else {
96 HandleTeleop(data);
97 }
Brian Silverman20141f92015-01-05 17:39:01 -080098 }
99 }
100
101 void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
Brian Silverman20141f92015-01-05 17:39:01 -0800102 const double wheel = -data.GetAxis(kSteeringWheel);
103 const double throttle = -data.GetAxis(kDriveThrottle);
Brian Silverman20141f92015-01-05 17:39:01 -0800104
Brian Silvermanada5f2c2015-02-01 02:41:14 -0500105 if (!drivetrain_queue.goal.MakeWithBuilder()
Brian Silverman20141f92015-01-05 17:39:01 -0800106 .steering(wheel)
107 .throttle(throttle)
Brian Silverman20141f92015-01-05 17:39:01 -0800108 .quickturn(data.IsPressed(kQuickTurn))
Austin Schuh331e13d2015-02-15 00:16:51 -0800109 .control_loop_driving(false)
Brian Silverman20141f92015-01-05 17:39:01 -0800110 .Send()) {
111 LOG(WARNING, "sending stick values failed\n");
112 }
Brian Silverman20141f92015-01-05 17:39:01 -0800113 }
114
115 void HandleTeleop(const ::aos::input::driver_station::Data &data) {
Brian Silverman20141f92015-01-05 17:39:01 -0800116 if (!data.GetControlBit(ControlBit::kEnabled)) {
117 action_queue_.CancelAllActions();
Austin Schuh6182f8d2015-02-14 22:15:04 -0800118 LOG(DEBUG, "Canceling\n");
Brian Silverman20141f92015-01-05 17:39:01 -0800119 }
120
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800121 if (data.PosEdge(kElevatorUp)) {
122 elevator_goal_ = 0.4;
123 arm_goal_ = 0.1;
124 claw_goal_ = 0.0;
125 }
126 if (data.PosEdge(kElevatorDown)) {
127 elevator_goal_ = 0.03;
128 arm_goal_ = 0.0;
129 claw_goal_ = 0.0;
130 }
131
132 if (data.PosEdge(kClawMiddle)) {
133 claw_goal_ = 0.9;
134 }
135
136 if (data.PosEdge(kClawClosed)) {
137 claw_rollers_closed_ = true;
138 }
139 if (data.PosEdge(kClawOpen)) {
140 claw_rollers_closed_ = false;
141 }
142
143 if (data.PosEdge(kFridgeClosed)) {
144 fridge_closed_ = true;
145 }
146 if (data.PosEdge(kFridgeOpen)) {
147 fridge_closed_ = false;
148 }
149
150 if (data.PosEdge(ControlBit::kEnabled)) {
151 // If we got enabled, wait for everything to zero.
152 LOG(INFO, "Waiting for zero.\n");
153 waiting_for_zero_ = true;
154 }
155
156 if (waiting_for_zero_) {
157 claw_queue.status.FetchLatest();
158 fridge_queue.status.FetchLatest();
159 if (!claw_queue.status.get()) {
160 LOG(ERROR, "Got no claw status packet.\n");
161 // Not safe to continue.
162 return;
163 }
164 if (!fridge_queue.status.get()) {
165 LOG(ERROR, "Got no fridge status packet.\n");
166 return;
167 }
168
169 if (claw_queue.status->zeroed && fridge_queue.status->zeroed) {
170 LOG(INFO, "Zeroed! Starting teleop mode.\n");
171 waiting_for_zero_ = false;
172
173 // Set the initial goals to where we are now.
174 elevator_goal_ = fridge_queue.status->goal_height;
175 arm_goal_ = fridge_queue.status->goal_angle;
176 claw_goal_ = claw_queue.status->angle;
177 } else {
178 return;
179 }
180 } else {
181 if (!action_queue_.Running()) {
182 auto new_fridge_goal = fridge_queue.goal.MakeMessage();
183 new_fridge_goal->height = elevator_goal_;
184 new_fridge_goal->angle = arm_goal_;
185 new_fridge_goal->angular_velocity = 0.0;
186 new_fridge_goal->velocity = 0.0;
187 new_fridge_goal->grabbers.top_front = fridge_closed_;
188 new_fridge_goal->grabbers.top_back = fridge_closed_;
189 new_fridge_goal->grabbers.bottom_front = fridge_closed_;
190 new_fridge_goal->grabbers.bottom_back = fridge_closed_;
191
192 if (!new_fridge_goal.Send()) {
193 LOG(ERROR, "Sending fridge goal failed.\n");
194 } else {
195 LOG(DEBUG, "sending goals: elevator: %f, arm: %f\n", elevator_goal_,
196 arm_goal_);
197 }
198 if (!claw_queue.goal.MakeWithBuilder()
199 .angle(claw_goal_)
200 .rollers_closed(claw_rollers_closed_)
201 .intake(data.IsPressed(kRollersIn) ? 12.0 : 0.0)
202 .Send()) {
203 LOG(ERROR, "Sending claw goal failed.\n");
204 }
205 }
206 }
207
Brian Silverman20141f92015-01-05 17:39:01 -0800208 action_queue_.Tick();
209 was_running_ = action_queue_.Running();
210 }
211
Daniel Petti61896522015-02-15 18:01:43 -0800212 void HandleTest(const ::aos::input::driver_station::Data &data) {
213 if (action_queue_.Running()) {
214 // We don't really want any actions running.
215 LOG(DEBUG, "Cancelling actions for test mode.\n");
216 action_queue_.CancelAllActions();
217 }
218
219 if (data.GetControlBit(ControlBit::kEnabled)) {
220 if (data.PosEdge(ControlBit::kEnabled)) {
221 // If we got enabled, wait for everything to zero.
222 LOG(INFO, "Waiting for zero.\n");
223 waiting_for_zero_ = true;
224 }
225 if (waiting_for_zero_) {
226 claw_queue.status.FetchLatest();
227 fridge_queue.status.FetchLatest();
228 if (!claw_queue.status.get()) {
229 LOG(ERROR, "Got no claw status packet.\n");
230 // Not safe to continue.
231 return;
232 }
233 if (!fridge_queue.status.get()) {
234 LOG(ERROR, "Got no fridge status packet.\n");
235 return;
236 }
237
238 if (claw_queue.status->zeroed && fridge_queue.status->zeroed) {
239 LOG(INFO, "Zeroed! Starting test mode.\n");
240 waiting_for_zero_ = false;
241
242 // Set the initial goals to where we are now.
243 elevator_goal_ = fridge_queue.status->height;
244 arm_goal_ = fridge_queue.status->angle;
245 claw_goal_ = claw_queue.status->angle;
246 } else {
247 return;
248 }
249 }
250
251 // These buttons move a subsystem up or down for as long as they are
252 // pressed, at low velocity.
253 if (data.IsPressed(kElevatorUp)) {
254 elevator_goal_ += kElevatorVelocity * kJoystickDt;
255 }
256 if (data.IsPressed(kElevatorDown)) {
257 elevator_goal_ -= kElevatorVelocity * kJoystickDt;
258 }
259 if (data.IsPressed(kArmUp)) {
260 arm_goal_ += kArmVelocity * kJoystickDt;
261 }
262 if (data.IsPressed(kArmDown)) {
263 arm_goal_ -= kArmVelocity * kJoystickDt;
264 }
265 if (data.IsPressed(kClawUp)) {
266 claw_goal_ += kClawVelocity * kJoystickDt;
267 }
268 if (data.IsPressed(kClawDown)) {
269 claw_goal_ -= kClawVelocity * kJoystickDt;
270 }
271
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -0800272 if (!action_queue_.Running()) {
273 if (!fridge_queue.goal.MakeWithBuilder()
274 .height(elevator_goal_)
275 .angle(arm_goal_)
276 .Send()) {
277 LOG(ERROR, "Sending fridge goal failed.\n");
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800278 } else {
279 LOG(DEBUG, "sending goals: elevator: %f, arm: %f\n", elevator_goal_,
280 arm_goal_);
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -0800281 }
282 if (!claw_queue.goal.MakeWithBuilder().angle(claw_goal_).Send()) {
283 LOG(ERROR, "Sending claw goal failed.\n");
284 }
Daniel Petti61896522015-02-15 18:01:43 -0800285 }
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800286 /*
Ben Fredrickson9fb2ab12015-02-16 16:42:08 -0800287 if (data.IsPressed(kArmPresetOne) || data.IsPressed(kArmPresetTwo)) {
288 actors::FridgeProfileParams fridge_params;
289 fridge_params.arm_max_velocity = kArmDebugVelocity;
290 fridge_params.arm_max_acceleration = kArmDebugAcceleration;
291 if (data.IsPressed(kArmPresetOne)) {
292 LOG(INFO, "Preset asked for test arm position one position.\n");
293 fridge_params.arm_angle = M_PI / 4.0;
294 fridge_params.top_front_grabber = false;
295 fridge_params.top_back_grabber = false;
296 fridge_params.bottom_front_grabber = false;
297 fridge_params.bottom_back_grabber = false;
298 action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
299 } else if (data.IsPressed(kArmPresetTwo)) {
300 LOG(INFO, "Preset asked for test arm position two position.\n");
301 fridge_params.arm_angle = -M_PI / 4.0;
302 fridge_params.top_front_grabber = true;
303 fridge_params.top_back_grabber = true;
304 fridge_params.bottom_front_grabber = true;
305 fridge_params.bottom_back_grabber = true;
306 action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
307 }
308 } else if (data.IsPressed(kElevatorPresetOne) ||
309 data.IsPressed(kElevatorPresetOne)) {
310 actors::FridgeProfileParams fridge_params;
311 fridge_params.elevator_max_velocity = kElevatorDebugVelocity;
312 fridge_params.elevator_max_acceleration = kElevatorDebugAcceleration;
313 if (data.IsPressed(kElevatorPresetOne)) {
314 LOG(INFO, "Preset asked for test elevator position one position.\n");
315 fridge_params.elevator_height = 0.5;
316 fridge_params.top_front_grabber = false;
317 fridge_params.top_back_grabber = false;
318 fridge_params.bottom_front_grabber = false;
319 fridge_params.bottom_back_grabber = false;
320 action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
321 } else if (data.IsPressed(kElevatorPresetTwo)) {
322 LOG(INFO, "Preset asked for test elevator position two position.\n");
323 fridge_params.elevator_height = 1.2;
324 fridge_params.top_front_grabber = true;
325 fridge_params.top_back_grabber = true;
326 fridge_params.bottom_front_grabber = true;
327 fridge_params.bottom_back_grabber = true;
328 action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
329 }
Daniel Petti61896522015-02-15 18:01:43 -0800330 }
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800331 */
Daniel Petti61896522015-02-15 18:01:43 -0800332 }
333 }
334
Brian Silverman20141f92015-01-05 17:39:01 -0800335 private:
Austin Schuh6182f8d2015-02-14 22:15:04 -0800336 void StartAuto() {
337 LOG(INFO, "Starting auto mode\n");
338 ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
339 }
340
341 void StopAuto() {
342 LOG(INFO, "Stopping auto mode\n");
343 ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
344 }
345
Brian Silverman20141f92015-01-05 17:39:01 -0800346 bool was_running_;
Daniel Petti61896522015-02-15 18:01:43 -0800347
348 // Previous goals for systems.
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800349 double elevator_goal_ = 0.2;
Daniel Petti61896522015-02-15 18:01:43 -0800350 double arm_goal_ = 0.0;
351 double claw_goal_ = 0.0;
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800352 bool claw_rollers_closed_ = false;
353 bool fridge_closed_ = false;
Daniel Petti61896522015-02-15 18:01:43 -0800354
355 // If we're waiting for the subsystems to zero.
Austin Schuhcde7ffe2015-02-20 22:10:41 -0800356 bool waiting_for_zero_ = true;
Daniel Petti61896522015-02-15 18:01:43 -0800357
Austin Schuh6182f8d2015-02-14 22:15:04 -0800358 bool auto_running_ = false;
359
Austin Schuh331e13d2015-02-15 00:16:51 -0800360 ::aos::common::actions::ActionQueue action_queue_;
Brian Silverman20141f92015-01-05 17:39:01 -0800361
362 ::aos::util::SimpleLogInterval no_drivetrain_status_ =
363 ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
364 "no drivetrain status");
365};
366
367} // namespace joysticks
368} // namespace input
369} // namespace frc971
370
371int main() {
372 ::aos::Init();
373 ::frc971::input::joysticks::Reader reader;
374 reader.Run();
375 ::aos::Cleanup();
376}