blob: 11b8a77ff4fdb2c17b9b46fc4c4a865230d29725 [file] [log] [blame]
Sabina Davis91b23602019-01-21 00:06:01 -08001#include <math.h>
2#include <stdio.h>
3#include <string.h>
4#include <unistd.h>
5
6#include "aos/actions/actions.h"
7#include "aos/init.h"
8#include "aos/input/action_joystick_input.h"
9#include "aos/input/driver_station_data.h"
10#include "aos/input/drivetrain_input.h"
11#include "aos/input/joystick_input.h"
12#include "aos/logging/logging.h"
13#include "aos/logging/logging.h"
Tyler Chatowe0241452019-03-08 21:07:50 -080014#include "aos/network/team_number.h"
Sabina Davis91b23602019-01-21 00:06:01 -080015#include "aos/util/log_interval.h"
Tyler Chatowe0241452019-03-08 21:07:50 -080016#include "aos/vision/events/udp.h"
17#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
Sabina Davis91b23602019-01-21 00:06:01 -080018#include "frc971/autonomous/auto.q.h"
19#include "frc971/autonomous/base_autonomous_actor.h"
20#include "frc971/control_loops/drivetrain/drivetrain.q.h"
James Kuszmauld8deb682019-03-10 10:38:42 -070021#include "frc971/control_loops/drivetrain/localizer.q.h"
Sabina Davis91b23602019-01-21 00:06:01 -080022
23#include "y2019/control_loops/drivetrain/drivetrain_base.h"
24#include "y2019/control_loops/superstructure/superstructure.q.h"
Sabina Davisc6329342019-03-01 20:44:42 -080025#include "y2019/status_light.q.h"
Tyler Chatowe0241452019-03-08 21:07:50 -080026#include "y2019/vision.pb.h"
Sabina Davis91b23602019-01-21 00:06:01 -080027
28using ::y2019::control_loops::superstructure::superstructure_queue;
James Kuszmauld8deb682019-03-10 10:38:42 -070029using ::frc971::control_loops::drivetrain::localizer_control;
Sabina Davis91b23602019-01-21 00:06:01 -080030using ::aos::input::driver_station::ButtonLocation;
31using ::aos::input::driver_station::ControlBit;
32using ::aos::input::driver_station::JoystickAxis;
33using ::aos::input::driver_station::POVLocation;
Tyler Chatowe0241452019-03-08 21:07:50 -080034using ::aos::events::ProtoTXUdpSocket;
Sabina Davis91b23602019-01-21 00:06:01 -080035
36namespace y2019 {
37namespace input {
38namespace joysticks {
39
Tyler Chatowe0241452019-03-08 21:07:50 -080040using google::protobuf::StringPrintf;
41
Austin Schuh1a17e132019-02-17 15:05:06 -080042const ButtonLocation kSuctionBall(3, 13);
43const ButtonLocation kSuctionHatch(3, 12);
44const ButtonLocation kDeployStilt(3, 8);
Sabina Davis069cbca2019-03-08 23:35:18 -080045const ButtonLocation kHalfStilt(3, 6);
Austin Schuh1a17e132019-02-17 15:05:06 -080046const ButtonLocation kFallOver(3, 9);
Austin Schuh2cf16b82019-02-15 23:23:22 -080047
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080048struct ElevatorWristPosition {
49 double elevator;
50 double wrist;
51};
Sabina Davis91b23602019-01-21 00:06:01 -080052
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080053const ButtonLocation kRocketForwardLower(5, 1);
54const ButtonLocation kRocketForwardMiddle(5, 2);
55const ButtonLocation kRocketForwardUpper(5, 4);
56const ButtonLocation kCargoForward(5, 3);
57
58const POVLocation kRocketBackwardUnpressed(5, -1);
59const POVLocation kRocketBackwardLower(5, 180);
60const POVLocation kRocketBackwardMiddle(5, 90);
61const POVLocation kRocketBackwardUpper(5, 0);
62const POVLocation kCargoBackward(5, 270);
63
64const ButtonLocation kPanelSwitch(5, 7);
65const ButtonLocation kCargoSwitch(5, 8);
66
67const ButtonLocation kBallHPIntakeForward(5, 6);
68const ButtonLocation kBallHPIntakeBackward(5, 5);
69const JoystickAxis kBallOutake(5, 3);
70const JoystickAxis kBallIntake(5, 4);
71
72const ButtonLocation kPanelHPIntakeForward(5, 6);
73const ButtonLocation kPanelHPIntakeBackward(5, 5);
74
75const ButtonLocation kRelease(2, 4);
James Kuszmauld8deb682019-03-10 10:38:42 -070076const ButtonLocation kResetLocalizer(4, 3);
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080077
78const ElevatorWristPosition kStowPos{0.36, 0.0};
79
80const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.04, M_PI / 2.0};
Sabina Davise6fe6c52019-03-03 15:48:51 -080081const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.05, -M_PI / 2.0};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080082
83const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0};
84const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0};
85
Sabina Davise48004f2019-03-02 23:15:24 -080086const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
87const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080088
Sabina Davise48004f2019-03-02 23:15:24 -080089const ElevatorWristPosition kPanelForwardUpperPos{1.51, M_PI / 2.0};
90const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080091
Sabina Davise6fe6c52019-03-03 15:48:51 -080092const ElevatorWristPosition kPanelCargoForwardPos{0.0, M_PI / 2.0};
93const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
94
95const ElevatorWristPosition kBallForwardLowerPos{0.46, M_PI / 2.0};
96const ElevatorWristPosition kBallBackwardLowerPos{0.15, -M_PI / 2.0};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -080097
Sabina Davise48004f2019-03-02 23:15:24 -080098const ElevatorWristPosition kBallForwardMiddlePos{1.16, 1.546};
99const ElevatorWristPosition kBallBackwardMiddlePos{0.876021, -1.546};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800100
Sabina Davise48004f2019-03-02 23:15:24 -0800101const ElevatorWristPosition kBallForwardUpperPos{1.50, 0.961};
102const ElevatorWristPosition kBallBackwardUpperPos{1.41, -1.217};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800103
Sabina Davise48004f2019-03-02 23:15:24 -0800104const ElevatorWristPosition kBallCargoForwardPos{0.699044, 1.353};
105const ElevatorWristPosition kBallCargoBackwardPos{0.828265, -1.999};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800106
Sabina Davise6fe6c52019-03-03 15:48:51 -0800107const ElevatorWristPosition kBallHPIntakeForwardPos{0.55, 1.097};
108const ElevatorWristPosition kBallHPIntakeBackwardPos{0.89, -2.018};
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800109
110const ElevatorWristPosition kBallIntakePos{0.29, 2.14};
Austin Schuh2cf16b82019-02-15 23:23:22 -0800111
Sabina Davis91b23602019-01-21 00:06:01 -0800112class Reader : public ::aos::input::ActionJoystickInput {
113 public:
114 Reader(::aos::EventLoop *event_loop)
115 : ::aos::input::ActionJoystickInput(
116 event_loop,
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800117 ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
Tyler Chatowe0241452019-03-08 21:07:50 -0800118 const uint16_t team = ::aos::network::GetTeamNumber();
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800119 superstructure_queue.goal.FetchLatest();
120 if (superstructure_queue.goal.get()) {
Sabina Davisc6329342019-03-01 20:44:42 -0800121 grab_piece_ = superstructure_queue.goal->suction.grab_piece;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800122 }
Tyler Chatowe0241452019-03-08 21:07:50 -0800123 video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
124 StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800125 }
Sabina Davis91b23602019-01-21 00:06:01 -0800126
127 void HandleTeleop(const ::aos::input::driver_station::Data &data) {
128 superstructure_queue.position.FetchLatest();
129 superstructure_queue.status.FetchLatest();
130 if (!superstructure_queue.status.get() ||
131 !superstructure_queue.position.get()) {
132 LOG(ERROR, "Got no superstructure status packet.\n");
133 return;
134 }
135
136 auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
137
James Kuszmauld8deb682019-03-10 10:38:42 -0700138 if (data.PosEdge(kResetLocalizer)) {
139 auto localizer_resetter = localizer_control.MakeMessage();
140 localizer_resetter->x = 0.4;
141 localizer_resetter->y = 3.4;
142 localizer_resetter->theta = 0.0;
143 if (!localizer_resetter.Send()) {
144 LOG(ERROR, "Failed to reset localizer.\n");
145 }
146 }
147
Austin Schuh1a17e132019-02-17 15:05:06 -0800148 if (data.IsPressed(kSuctionBall)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800149 grab_piece_ = true;
Austin Schuh1a17e132019-02-17 15:05:06 -0800150 } else if (data.IsPressed(kSuctionHatch)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800151 grab_piece_ = true;
Austin Schuh1a17e132019-02-17 15:05:06 -0800152 } else if (data.IsPressed(kRelease) ||
153 !superstructure_queue.status->has_piece) {
Sabina Davisc6329342019-03-01 20:44:42 -0800154 grab_piece_ = false;
Austin Schuh1a17e132019-02-17 15:05:06 -0800155 }
Sabina Davis91b23602019-01-21 00:06:01 -0800156
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800157 if (data.IsPressed(kRocketBackwardUnpressed)) {
158 elevator_wrist_pos_ = kStowPos;
159 }
160 new_superstructure_goal->intake.unsafe_goal = -1.2;
161 new_superstructure_goal->roller_voltage = 0.0;
162
163 const bool kDoBallIntake = data.GetAxis(kBallIntake) > 0.9;
164 const bool kDoBallOutake = data.GetAxis(kBallOutake) > 0.9;
165
166 if (data.IsPressed(kPanelSwitch)) {
167 switch_ball_ = false;
168 } else if (data.IsPressed(kCargoSwitch)) {
169 switch_ball_ = true;
170 }
171
Sabina Davis91b23602019-01-21 00:06:01 -0800172 // TODO(sabina): max height please?
Austin Schuh77ac3212019-02-19 16:50:14 -0800173 if (data.IsPressed(kFallOver)) {
Austin Schuh1a17e132019-02-17 15:05:06 -0800174 new_superstructure_goal->stilts.unsafe_goal = 0.71;
Michael Schuh587dcb52019-02-28 21:31:03 -0800175 new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800176 new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
Austin Schuh77ac3212019-02-19 16:50:14 -0800177 } else if (data.IsPressed(kDeployStilt)) {
178 new_superstructure_goal->stilts.unsafe_goal = 0.50;
Michael Schuh587dcb52019-02-28 21:31:03 -0800179 new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800180 if (stilts_was_above_) {
181 new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
182 } else {
183 new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
184 }
Sabina Davis069cbca2019-03-08 23:35:18 -0800185 } else if (data.IsPressed(kHalfStilt)) {
186 new_superstructure_goal->stilts.unsafe_goal = 0.345;
187 new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
188 new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
Sabina Davis91b23602019-01-21 00:06:01 -0800189 } else {
Sabina Davisbfdeb362019-03-09 16:20:01 -0800190 new_superstructure_goal->stilts.unsafe_goal = 0.005;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800191 new_superstructure_goal->stilts.profile_params.max_velocity = 0.25;
192 new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
Sabina Davis91b23602019-01-21 00:06:01 -0800193 }
194
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800195 if (superstructure_queue.status->stilts.position > 0.65) {
196 stilts_was_above_ = true;
197 } else if (superstructure_queue.status->stilts.position < 0.1) {
198 stilts_was_above_ = false;
Austin Schuh2cf16b82019-02-15 23:23:22 -0800199 }
200
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800201 if (switch_ball_) {
202 if (superstructure_queue.status->has_piece) {
203 new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
Austin Schuh1a17e132019-02-17 15:05:06 -0800204 }
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800205
206 // Go to intake position and apply vacuum
207 if (data.IsPressed(kBallHPIntakeForward)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800208 grab_piece_ = true;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800209 elevator_wrist_pos_ = kBallHPIntakeForwardPos;
210 } else if (data.IsPressed(kBallHPIntakeBackward)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800211 grab_piece_ = true;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800212 elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
213 }
214
215 // Go to elevator/wrist position. Overrides intake position if pressed so
216 // we can re-grab the ball.
217 if (data.IsPressed(kRocketForwardLower)) {
218 elevator_wrist_pos_ = kBallForwardLowerPos;
219 } else if (data.IsPressed(kRocketBackwardLower)) {
220 elevator_wrist_pos_ = kBallBackwardLowerPos;
221 } else if (data.IsPressed(kRocketForwardMiddle)) {
222 elevator_wrist_pos_ = kBallForwardMiddlePos;
223 } else if (data.IsPressed(kRocketBackwardMiddle)) {
224 elevator_wrist_pos_ = kBallBackwardMiddlePos;
225 } else if (data.IsPressed(kRocketForwardUpper)) {
226 elevator_wrist_pos_ = kBallForwardUpperPos;
227 } else if (data.IsPressed(kRocketBackwardUpper)) {
228 elevator_wrist_pos_ = kBallBackwardUpperPos;
229 } else if (data.IsPressed(kCargoForward)) {
230 elevator_wrist_pos_ = kBallCargoForwardPos;
231 } else if (data.IsPressed(kCargoBackward)) {
232 elevator_wrist_pos_ = kBallCargoBackwardPos;
233 }
Austin Schuh1a17e132019-02-17 15:05:06 -0800234 } else {
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800235 if (data.IsPressed(kPanelHPIntakeForward)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800236 grab_piece_ = true;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800237 elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
238 } else if (data.IsPressed(kPanelHPIntakeBackward)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800239 grab_piece_ = true;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800240 elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
241 }
242
243 // Go to elevator/wrist position. Overrides intake position if pressed so
244 // we can re-grab the panel.
245 if (data.IsPressed(kRocketForwardLower)) {
246 elevator_wrist_pos_ = kPanelForwardLowerPos;
247 } else if (data.IsPressed(kRocketBackwardLower)) {
248 elevator_wrist_pos_ = kPanelBackwardLowerPos;
249 } else if (data.IsPressed(kRocketForwardMiddle)) {
250 elevator_wrist_pos_ = kPanelForwardMiddlePos;
251 } else if (data.IsPressed(kRocketBackwardMiddle)) {
252 elevator_wrist_pos_ = kPanelBackwardMiddlePos;
253 } else if (data.IsPressed(kRocketForwardUpper)) {
254 elevator_wrist_pos_ = kPanelForwardUpperPos;
255 } else if (data.IsPressed(kRocketBackwardUpper)) {
256 elevator_wrist_pos_ = kPanelBackwardUpperPos;
Sabina Davise6fe6c52019-03-03 15:48:51 -0800257 } else if (data.IsPressed(kCargoForward)) {
258 elevator_wrist_pos_ = kPanelCargoForwardPos;
259 } else if (data.IsPressed(kCargoBackward)) {
260 elevator_wrist_pos_ = kPanelCargoBackwardPos;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800261 }
Austin Schuh1a17e132019-02-17 15:05:06 -0800262 }
263
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800264 if (switch_ball_) {
265 if (kDoBallOutake ||
266 (kDoBallIntake && !superstructure_queue.status->has_piece)) {
267 new_superstructure_goal->intake.unsafe_goal = 0.959327;
268 }
Austin Schuh23a51632019-02-19 16:50:36 -0800269
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800270 if (kDoBallIntake && !superstructure_queue.status->has_piece) {
271 elevator_wrist_pos_ = kBallIntakePos;
272 new_superstructure_goal->roller_voltage = 9.0;
Sabina Davisc6329342019-03-01 20:44:42 -0800273 grab_piece_ = true;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800274 } else {
275 if (kDoBallOutake) {
276 new_superstructure_goal->roller_voltage = -6.0;
277 } else {
278 new_superstructure_goal->intake.unsafe_goal = -1.2;
279 new_superstructure_goal->roller_voltage = 0.0;
280 }
281 }
Austin Schuh23a51632019-02-19 16:50:36 -0800282 }
Austin Schuh1a17e132019-02-17 15:05:06 -0800283
284 if (data.IsPressed(kRelease)) {
Sabina Davisc6329342019-03-01 20:44:42 -0800285 grab_piece_ = false;
Austin Schuh1a17e132019-02-17 15:05:06 -0800286 }
287
Sabina Davisc6329342019-03-01 20:44:42 -0800288 if (switch_ball_) {
289 new_superstructure_goal->suction.gamepiece_mode = 0;
290 } else {
291 new_superstructure_goal->suction.gamepiece_mode = 1;
292 }
293
Tyler Chatowe0241452019-03-08 21:07:50 -0800294 vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0);
295
Sabina Davisc6329342019-03-01 20:44:42 -0800296 new_superstructure_goal->suction.grab_piece = grab_piece_;
Sabina Davis91b23602019-01-21 00:06:01 -0800297
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800298 new_superstructure_goal->elevator.unsafe_goal =
299 elevator_wrist_pos_.elevator;
300 new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist;
Sabina Davis91b23602019-01-21 00:06:01 -0800301
302 LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
303 if (!new_superstructure_goal.Send()) {
304 LOG(ERROR, "Sending superstructure goal failed.\n");
305 }
Tyler Chatowe0241452019-03-08 21:07:50 -0800306
307 video_tx_->Send(vision_control_);
Sabina Davis91b23602019-01-21 00:06:01 -0800308 }
309
310 private:
311 // Current goals here.
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800312 ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
Sabina Davisc6329342019-03-01 20:44:42 -0800313 bool grab_piece_ = false;
Tyler Chatow7bcb52f2019-02-24 00:16:54 -0800314
315 bool switch_ball_ = false;
316 bool stilts_was_above_ = false;
Tyler Chatowe0241452019-03-08 21:07:50 -0800317
318 VisionControl vision_control_;
319 ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
Sabina Davis91b23602019-01-21 00:06:01 -0800320};
321
322} // namespace joysticks
323} // namespace input
324} // namespace y2019
325
326int main() {
327 ::aos::Init(-1);
328 ::aos::ShmEventLoop event_loop;
329 ::y2019::input::joysticks::Reader reader(&event_loop);
330 reader.Run();
331 ::aos::Cleanup();
332}