blob: 89de4b97ae840cf55db7d463eb4a2cdc24bb5650 [file] [log] [blame]
Stephan Massaltd021f972020-01-05 20:41:23 -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/network/team_number.h"
14#include "aos/util/log_interval.h"
15#include "frc971/autonomous/base_autonomous_actor.h"
16#include "y2020/control_loops/drivetrain/drivetrain_base.h"
Sabina Davisa8fed3d2020-02-22 21:44:57 -080017#include "y2020/constants.h"
Stephan Massaltd021f972020-01-05 20:41:23 -080018#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
19#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
20
21using aos::input::driver_station::ButtonLocation;
22using aos::input::driver_station::ControlBit;
23using aos::input::driver_station::JoystickAxis;
24using aos::input::driver_station::POVLocation;
25
Austin Schuhf0a637c2020-02-25 23:44:12 -080026using frc971::CreateProfileParameters;
Sabina Davisa8fed3d2020-02-22 21:44:57 -080027using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
28using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
29
30
Stephan Massaltd021f972020-01-05 20:41:23 -080031namespace y2020 {
32namespace input {
33namespace joysticks {
34
35namespace superstructure = y2020::control_loops::superstructure;
36
Sabina Davisa8fed3d2020-02-22 21:44:57 -080037// TODO(sabina): fix button locations.
38
Austin Schuhf0a637c2020-02-25 23:44:12 -080039const ButtonLocation kShootFast(3, 16);
40const ButtonLocation kTurret(3, 15);
41const ButtonLocation kHood(3, 3);
Sabina Davisa8fed3d2020-02-22 21:44:57 -080042const ButtonLocation kShootSlow(4, 2);
Austin Schuh43a220f2020-02-26 22:02:34 -080043const ButtonLocation kFeed(4, 1);
Austin Schuhf0a637c2020-02-25 23:44:12 -080044const ButtonLocation kIntakeExtend(3, 9);
Sabina Davisa8fed3d2020-02-22 21:44:57 -080045const ButtonLocation kIntakeIn(4, 4);
46const ButtonLocation kSpit(4, 5);
47
Stephan Massaltd021f972020-01-05 20:41:23 -080048class Reader : public ::aos::input::ActionJoystickInput {
49 public:
50 Reader(::aos::EventLoop *event_loop)
51 : ::aos::input::ActionJoystickInput(
52 event_loop,
53 ::y2020::control_loops::drivetrain::GetDrivetrainConfig(),
54 ::aos::input::DrivetrainInputReader::InputType::kPistol, {}),
55 superstructure_goal_sender_(
56 event_loop->MakeSender<superstructure::Goal>("/superstructure")),
57 superstructure_status_fetcher_(
58 event_loop->MakeFetcher<superstructure::Status>(
59 "/superstructure")) {}
60
61 void AutoEnded() override {
62 AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
63 }
64
Sabina Davisa8fed3d2020-02-22 21:44:57 -080065 void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
Stephan Massaltd021f972020-01-05 20:41:23 -080066 superstructure_status_fetcher_.Fetch();
67 if (!superstructure_status_fetcher_.get()) {
68 AOS_LOG(ERROR, "Got no superstructure status message.\n");
69 return;
70 }
Sabina Davisa8fed3d2020-02-22 21:44:57 -080071
Austin Schuhf0a637c2020-02-25 23:44:12 -080072 double hood_pos = constants::Values::kHoodRange().middle();
73 double intake_pos = -0.89;
Sabina Davisa8fed3d2020-02-22 21:44:57 -080074 double turret_pos = 0.0;
75 float roller_speed = 0.0f;
76 double accelerator_speed = 0.0;
77 double finisher_speed = 0.0;
78
Austin Schuhf0a637c2020-02-25 23:44:12 -080079 if (data.IsPressed(kTurret)) {
80 turret_pos = 3.5;
Sabina Davisa8fed3d2020-02-22 21:44:57 -080081 }
82
Austin Schuhf0a637c2020-02-25 23:44:12 -080083 if (data.IsPressed(kHood)) {
84 hood_pos = 0.05;
85 }
86
87 if (data.IsPressed(kShootFast)) {
Austin Schuh43a220f2020-02-26 22:02:34 -080088 accelerator_speed = 500.0;
89 finisher_speed = 500.0;
Austin Schuhf0a637c2020-02-25 23:44:12 -080090 } else if (data.IsPressed(kShootSlow)) {
Sabina Davisa8fed3d2020-02-22 21:44:57 -080091 accelerator_speed = 30.0;
92 finisher_speed = 30.0;
93 }
94
95 if (data.IsPressed(kIntakeExtend)) {
Austin Schuhf0a637c2020-02-25 23:44:12 -080096 intake_pos = 1.0;
Sabina Davisa8fed3d2020-02-22 21:44:57 -080097 }
98
Austin Schuh43a220f2020-02-26 22:02:34 -080099 if (data.IsPressed(kFeed)) {
100 }
101
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800102 if (data.IsPressed(kIntakeIn)) {
103 roller_speed = 6.0f;
104 } else if (data.IsPressed(kSpit)) {
105 roller_speed = -6.0f;
106 }
107
108 auto builder = superstructure_goal_sender_.MakeBuilder();
109
110 flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
111 {
112 flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
113 hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
Austin Schuhf0a637c2020-02-25 23:44:12 -0800114 *builder.fbb(), hood_pos,
115 CreateProfileParameters(*builder.fbb(), 0.5, 1.0));
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800116
117 flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
118 intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
Austin Schuhf0a637c2020-02-25 23:44:12 -0800119 *builder.fbb(), intake_pos,
120 CreateProfileParameters(*builder.fbb(), 10.0, 30.0));
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800121
122 flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
123 turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
Austin Schuhf0a637c2020-02-25 23:44:12 -0800124 *builder.fbb(), turret_pos,
125 CreateProfileParameters(*builder.fbb(), 6.0, 20.0));
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800126
127 flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
Austin Schuhf0a637c2020-02-25 23:44:12 -0800128 superstructure::CreateShooterGoal(*builder.fbb(), accelerator_speed,
129 finisher_speed);
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800130
131 superstructure::Goal::Builder superstructure_goal_builder =
132 builder.MakeBuilder<superstructure::Goal>();
133
134 superstructure_goal_builder.add_hood(hood_offset);
135 superstructure_goal_builder.add_intake(intake_offset);
136 superstructure_goal_builder.add_turret(turret_offset);
137 superstructure_goal_builder.add_roller_voltage(roller_speed);
138 superstructure_goal_builder.add_shooter(shooter_offset);
Austin Schuh43a220f2020-02-26 22:02:34 -0800139 superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
Sabina Davisa8fed3d2020-02-22 21:44:57 -0800140
141 if (!builder.Send(superstructure_goal_builder.Finish())) {
142 AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
143 }
144 }
Stephan Massaltd021f972020-01-05 20:41:23 -0800145 }
146
147 private:
148 ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
149
150 ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
151};
152
153} // namespace joysticks
154} // namespace input
155} // namespace y2020
156
157int main() {
James Kuszmaulad8a8082020-02-14 21:21:58 -0800158 ::aos::InitNRT();
Stephan Massaltd021f972020-01-05 20:41:23 -0800159
160 aos::FlatbufferDetachedBuffer<aos::Configuration> config =
161 aos::configuration::ReadConfig("config.json");
162
163 ::aos::ShmEventLoop event_loop(&config.message());
164 ::y2020::input::joysticks::Reader reader(&event_loop);
165
166 event_loop.Run();
167
168 ::aos::Cleanup();
169}