Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 1 | #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 Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 14 | #include "aos/network/team_number.h" |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 15 | #include "aos/util/log_interval.h" |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 16 | #include "aos/vision/events/udp.h" |
| 17 | #include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h" |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 18 | #include "frc971/autonomous/auto.q.h" |
| 19 | #include "frc971/autonomous/base_autonomous_actor.h" |
| 20 | #include "frc971/control_loops/drivetrain/drivetrain.q.h" |
James Kuszmaul | d8deb68 | 2019-03-10 10:38:42 -0700 | [diff] [blame] | 21 | #include "frc971/control_loops/drivetrain/localizer.q.h" |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 22 | |
| 23 | #include "y2019/control_loops/drivetrain/drivetrain_base.h" |
| 24 | #include "y2019/control_loops/superstructure/superstructure.q.h" |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 25 | #include "y2019/status_light.q.h" |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 26 | #include "y2019/vision.pb.h" |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 27 | |
| 28 | using ::y2019::control_loops::superstructure::superstructure_queue; |
James Kuszmaul | d8deb68 | 2019-03-10 10:38:42 -0700 | [diff] [blame] | 29 | using ::frc971::control_loops::drivetrain::localizer_control; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 30 | using ::aos::input::driver_station::ButtonLocation; |
| 31 | using ::aos::input::driver_station::ControlBit; |
| 32 | using ::aos::input::driver_station::JoystickAxis; |
| 33 | using ::aos::input::driver_station::POVLocation; |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 34 | using ::aos::events::ProtoTXUdpSocket; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 35 | |
| 36 | namespace y2019 { |
| 37 | namespace input { |
| 38 | namespace joysticks { |
| 39 | |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 40 | using google::protobuf::StringPrintf; |
| 41 | |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 42 | const ButtonLocation kSuctionBall(3, 13); |
| 43 | const ButtonLocation kSuctionHatch(3, 12); |
| 44 | const ButtonLocation kDeployStilt(3, 8); |
Sabina Davis | 069cbca | 2019-03-08 23:35:18 -0800 | [diff] [blame] | 45 | const ButtonLocation kHalfStilt(3, 6); |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 46 | const ButtonLocation kFallOver(3, 9); |
Austin Schuh | 2cf16b8 | 2019-02-15 23:23:22 -0800 | [diff] [blame] | 47 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 48 | struct ElevatorWristPosition { |
| 49 | double elevator; |
| 50 | double wrist; |
| 51 | }; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 52 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 53 | const ButtonLocation kRocketForwardLower(5, 1); |
| 54 | const ButtonLocation kRocketForwardMiddle(5, 2); |
| 55 | const ButtonLocation kRocketForwardUpper(5, 4); |
| 56 | const ButtonLocation kCargoForward(5, 3); |
| 57 | |
| 58 | const POVLocation kRocketBackwardUnpressed(5, -1); |
| 59 | const POVLocation kRocketBackwardLower(5, 180); |
| 60 | const POVLocation kRocketBackwardMiddle(5, 90); |
| 61 | const POVLocation kRocketBackwardUpper(5, 0); |
| 62 | const POVLocation kCargoBackward(5, 270); |
| 63 | |
| 64 | const ButtonLocation kPanelSwitch(5, 7); |
| 65 | const ButtonLocation kCargoSwitch(5, 8); |
| 66 | |
| 67 | const ButtonLocation kBallHPIntakeForward(5, 6); |
| 68 | const ButtonLocation kBallHPIntakeBackward(5, 5); |
| 69 | const JoystickAxis kBallOutake(5, 3); |
| 70 | const JoystickAxis kBallIntake(5, 4); |
| 71 | |
| 72 | const ButtonLocation kPanelHPIntakeForward(5, 6); |
| 73 | const ButtonLocation kPanelHPIntakeBackward(5, 5); |
| 74 | |
| 75 | const ButtonLocation kRelease(2, 4); |
James Kuszmaul | d8deb68 | 2019-03-10 10:38:42 -0700 | [diff] [blame] | 76 | const ButtonLocation kResetLocalizer(4, 3); |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 77 | |
| 78 | const ElevatorWristPosition kStowPos{0.36, 0.0}; |
| 79 | |
| 80 | const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.04, M_PI / 2.0}; |
Sabina Davis | e6fe6c5 | 2019-03-03 15:48:51 -0800 | [diff] [blame] | 81 | const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.05, -M_PI / 2.0}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 82 | |
| 83 | const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0}; |
| 84 | const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0}; |
| 85 | |
Sabina Davis | e48004f | 2019-03-02 23:15:24 -0800 | [diff] [blame] | 86 | const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0}; |
| 87 | const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 88 | |
Sabina Davis | e48004f | 2019-03-02 23:15:24 -0800 | [diff] [blame] | 89 | const ElevatorWristPosition kPanelForwardUpperPos{1.51, M_PI / 2.0}; |
| 90 | const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 91 | |
Sabina Davis | e6fe6c5 | 2019-03-03 15:48:51 -0800 | [diff] [blame] | 92 | const ElevatorWristPosition kPanelCargoForwardPos{0.0, M_PI / 2.0}; |
| 93 | const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0}; |
| 94 | |
| 95 | const ElevatorWristPosition kBallForwardLowerPos{0.46, M_PI / 2.0}; |
| 96 | const ElevatorWristPosition kBallBackwardLowerPos{0.15, -M_PI / 2.0}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 97 | |
Sabina Davis | e48004f | 2019-03-02 23:15:24 -0800 | [diff] [blame] | 98 | const ElevatorWristPosition kBallForwardMiddlePos{1.16, 1.546}; |
| 99 | const ElevatorWristPosition kBallBackwardMiddlePos{0.876021, -1.546}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 100 | |
Sabina Davis | e48004f | 2019-03-02 23:15:24 -0800 | [diff] [blame] | 101 | const ElevatorWristPosition kBallForwardUpperPos{1.50, 0.961}; |
| 102 | const ElevatorWristPosition kBallBackwardUpperPos{1.41, -1.217}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 103 | |
Sabina Davis | e48004f | 2019-03-02 23:15:24 -0800 | [diff] [blame] | 104 | const ElevatorWristPosition kBallCargoForwardPos{0.699044, 1.353}; |
| 105 | const ElevatorWristPosition kBallCargoBackwardPos{0.828265, -1.999}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 106 | |
Sabina Davis | e6fe6c5 | 2019-03-03 15:48:51 -0800 | [diff] [blame] | 107 | const ElevatorWristPosition kBallHPIntakeForwardPos{0.55, 1.097}; |
| 108 | const ElevatorWristPosition kBallHPIntakeBackwardPos{0.89, -2.018}; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 109 | |
| 110 | const ElevatorWristPosition kBallIntakePos{0.29, 2.14}; |
Austin Schuh | 2cf16b8 | 2019-02-15 23:23:22 -0800 | [diff] [blame] | 111 | |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 112 | class Reader : public ::aos::input::ActionJoystickInput { |
| 113 | public: |
| 114 | Reader(::aos::EventLoop *event_loop) |
| 115 | : ::aos::input::ActionJoystickInput( |
| 116 | event_loop, |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 117 | ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) { |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 118 | const uint16_t team = ::aos::network::GetTeamNumber(); |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 119 | superstructure_queue.goal.FetchLatest(); |
| 120 | if (superstructure_queue.goal.get()) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 121 | grab_piece_ = superstructure_queue.goal->suction.grab_piece; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 122 | } |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 123 | video_tx_.reset(new ProtoTXUdpSocket<VisionControl>( |
| 124 | StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000)); |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 125 | } |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 126 | |
| 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 Kuszmaul | d8deb68 | 2019-03-10 10:38:42 -0700 | [diff] [blame] | 138 | 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 Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 148 | if (data.IsPressed(kSuctionBall)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 149 | grab_piece_ = true; |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 150 | } else if (data.IsPressed(kSuctionHatch)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 151 | grab_piece_ = true; |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 152 | } else if (data.IsPressed(kRelease) || |
| 153 | !superstructure_queue.status->has_piece) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 154 | grab_piece_ = false; |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 155 | } |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 156 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 157 | 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 Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 172 | // TODO(sabina): max height please? |
Austin Schuh | 77ac321 | 2019-02-19 16:50:14 -0800 | [diff] [blame] | 173 | if (data.IsPressed(kFallOver)) { |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 174 | new_superstructure_goal->stilts.unsafe_goal = 0.71; |
Michael Schuh | 587dcb5 | 2019-02-28 21:31:03 -0800 | [diff] [blame] | 175 | new_superstructure_goal->stilts.profile_params.max_velocity = 0.65; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 176 | new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0; |
Austin Schuh | 77ac321 | 2019-02-19 16:50:14 -0800 | [diff] [blame] | 177 | } else if (data.IsPressed(kDeployStilt)) { |
| 178 | new_superstructure_goal->stilts.unsafe_goal = 0.50; |
Michael Schuh | 587dcb5 | 2019-02-28 21:31:03 -0800 | [diff] [blame] | 179 | new_superstructure_goal->stilts.profile_params.max_velocity = 0.65; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 180 | 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 Davis | 069cbca | 2019-03-08 23:35:18 -0800 | [diff] [blame] | 185 | } 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 Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 189 | } else { |
Sabina Davis | bfdeb36 | 2019-03-09 16:20:01 -0800 | [diff] [blame] | 190 | new_superstructure_goal->stilts.unsafe_goal = 0.005; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 191 | new_superstructure_goal->stilts.profile_params.max_velocity = 0.25; |
| 192 | new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 193 | } |
| 194 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 195 | 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 Schuh | 2cf16b8 | 2019-02-15 23:23:22 -0800 | [diff] [blame] | 199 | } |
| 200 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 201 | if (switch_ball_) { |
| 202 | if (superstructure_queue.status->has_piece) { |
| 203 | new_superstructure_goal->wrist.profile_params.max_acceleration = 20; |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 204 | } |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 205 | |
| 206 | // Go to intake position and apply vacuum |
| 207 | if (data.IsPressed(kBallHPIntakeForward)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 208 | grab_piece_ = true; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 209 | elevator_wrist_pos_ = kBallHPIntakeForwardPos; |
| 210 | } else if (data.IsPressed(kBallHPIntakeBackward)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 211 | grab_piece_ = true; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 212 | 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 Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 234 | } else { |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 235 | if (data.IsPressed(kPanelHPIntakeForward)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 236 | grab_piece_ = true; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 237 | elevator_wrist_pos_ = kPanelHPIntakeForwrdPos; |
| 238 | } else if (data.IsPressed(kPanelHPIntakeBackward)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 239 | grab_piece_ = true; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 240 | 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 Davis | e6fe6c5 | 2019-03-03 15:48:51 -0800 | [diff] [blame] | 257 | } else if (data.IsPressed(kCargoForward)) { |
| 258 | elevator_wrist_pos_ = kPanelCargoForwardPos; |
| 259 | } else if (data.IsPressed(kCargoBackward)) { |
| 260 | elevator_wrist_pos_ = kPanelCargoBackwardPos; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 261 | } |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 262 | } |
| 263 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 264 | if (switch_ball_) { |
| 265 | if (kDoBallOutake || |
| 266 | (kDoBallIntake && !superstructure_queue.status->has_piece)) { |
| 267 | new_superstructure_goal->intake.unsafe_goal = 0.959327; |
| 268 | } |
Austin Schuh | 23a5163 | 2019-02-19 16:50:36 -0800 | [diff] [blame] | 269 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 270 | if (kDoBallIntake && !superstructure_queue.status->has_piece) { |
| 271 | elevator_wrist_pos_ = kBallIntakePos; |
| 272 | new_superstructure_goal->roller_voltage = 9.0; |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 273 | grab_piece_ = true; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 274 | } 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 Schuh | 23a5163 | 2019-02-19 16:50:36 -0800 | [diff] [blame] | 282 | } |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 283 | |
| 284 | if (data.IsPressed(kRelease)) { |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 285 | grab_piece_ = false; |
Austin Schuh | 1a17e13 | 2019-02-17 15:05:06 -0800 | [diff] [blame] | 286 | } |
| 287 | |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 288 | if (switch_ball_) { |
| 289 | new_superstructure_goal->suction.gamepiece_mode = 0; |
| 290 | } else { |
| 291 | new_superstructure_goal->suction.gamepiece_mode = 1; |
| 292 | } |
| 293 | |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 294 | vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0); |
| 295 | |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 296 | new_superstructure_goal->suction.grab_piece = grab_piece_; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 297 | |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 298 | new_superstructure_goal->elevator.unsafe_goal = |
| 299 | elevator_wrist_pos_.elevator; |
| 300 | new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 301 | |
| 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 Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 306 | |
| 307 | video_tx_->Send(vision_control_); |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 308 | } |
| 309 | |
| 310 | private: |
| 311 | // Current goals here. |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 312 | ElevatorWristPosition elevator_wrist_pos_ = kStowPos; |
Sabina Davis | c632934 | 2019-03-01 20:44:42 -0800 | [diff] [blame] | 313 | bool grab_piece_ = false; |
Tyler Chatow | 7bcb52f | 2019-02-24 00:16:54 -0800 | [diff] [blame] | 314 | |
| 315 | bool switch_ball_ = false; |
| 316 | bool stilts_was_above_ = false; |
Tyler Chatow | e024145 | 2019-03-08 21:07:50 -0800 | [diff] [blame] | 317 | |
| 318 | VisionControl vision_control_; |
| 319 | ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_; |
Sabina Davis | 91b2360 | 2019-01-21 00:06:01 -0800 | [diff] [blame] | 320 | }; |
| 321 | |
| 322 | } // namespace joysticks |
| 323 | } // namespace input |
| 324 | } // namespace y2019 |
| 325 | |
| 326 | int 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 | } |