Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/joystick_reader.cc b/y2015/joystick_reader.cc
new file mode 100644
index 0000000..01b5431
--- /dev/null
+++ b/y2015/joystick_reader.cc
@@ -0,0 +1,549 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+#include "aos/common/actions/actions.h"
+
+#include "y2015/control_loops/claw/claw.q.h"
+#include "y2015/control_loops/drivetrain/drivetrain.q.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+#include "y2015/constants.h"
+#include "frc971/queues/gyro.q.h"
+#include "frc971/autonomous/auto.q.h"
+#include "y2015/actors/pickup_actor.h"
+#include "y2015/actors/stack_actor.h"
+#include "y2015/actors/score_actor.h"
+#include "y2015/actors/stack_and_lift_actor.h"
+#include "y2015/actors/stack_and_hold_actor.h"
+#include "y2015/actors/held_to_lift_actor.h"
+#include "y2015/actors/lift_actor.h"
+#include "y2015/actors/can_pickup_actor.h"
+#include "y2015/actors/horizontal_can_pickup_actor.h"
+
+using ::frc971::control_loops::claw_queue;
+using ::frc971::control_loops::drivetrain_queue;
+using ::frc971::control_loops::fridge_queue;
+using ::frc971::sensors::gyro_reading;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::POVLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace frc971 {
+namespace input {
+namespace joysticks {
+
+// Actions needed.
+
+// Human Player Station Intake Button
+// Claw open
+// Claw close
+// Claw down
+
+// Stack + Lift (together)
+// Place
+
+// Hold stack
+
+// Horizontal can pickup
+// Vertical can pickup
+
+// TODO(austin): Pull a lot of the constants below out up here so they can be
+// globally changed easier.
+
+// preset motion limits
+constexpr actors::ProfileParams kArmMove{0.5, 1.0};
+constexpr actors::ProfileParams kElevatorMove{0.3, 1.0};
+
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+//const ButtonLocation kClawClosed(3, 5);
+//const ButtonLocation kFridgeClosed(3, 1);
+
+
+const ButtonLocation kStackAndHold(3, 5);
+const ButtonLocation kRollersIn(3, 2);
+const ButtonLocation kHorizontalCanRollersIn(4, 5);
+const ButtonLocation kClawToggle(4, 1);
+
+const POVLocation kElevatorCanUp(4, 0);
+
+// POV stick 3, 180
+const POVLocation kCanPickup(4, 180);
+const ButtonLocation kToteChute(4, 6);
+const ButtonLocation kStackAndLift(4, 7);
+
+// Pull in the 6th tote.
+//const ButtonLocation kSixthTote(4, 10);
+const ButtonLocation kCanUp(4, 10);
+
+const ButtonLocation kHeldToLift(4, 11);
+const ButtonLocation kPickup(4, 9);
+
+const ButtonLocation kStack(4, 2);
+
+// Move the fridge out with the stack in preparation for scoring.
+const ButtonLocation kScore(4, 8);
+const ButtonLocation kCoopTop(3, 8);
+const ButtonLocation kCoopTopRetract(3, 7);
+const ButtonLocation kCoopBottom(3, 6);
+const ButtonLocation kCoopBottomRetract(4, 12);
+
+const ButtonLocation kCanReset(3, 9);
+
+const POVLocation kFridgeToggle(4, 270);
+const ButtonLocation kSpit(4, 3);
+
+// Set stack down in the bot.
+const POVLocation kSetStackDownAndHold(4, 90);
+
+const double kClawTotePackAngle = 0.90;
+const double kArmRaiseLowerClearance = -0.08;
+const double kClawStackClearance = 0.55;
+const double kHorizontalCanClawAngle = 0.12;
+
+const double kStackUpHeight = 0.60;
+const double kStackUpArm = 0.0;
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() : was_running_(false) {}
+
+ static actors::ScoreParams MakeScoreParams() {
+ actors::ScoreParams r;
+ r.move_the_stack = r.place_the_stack = true;
+ r.upper_move_height = 0.14;
+ r.begin_horizontal_move_height = 0.13;
+ r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = -0.65;
+ r.home_lift_horizontal_start_position = -0.60;
+ r.place_height = -0.10;
+ r.home_return_height = 0.05;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopTopParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.move_the_stack = !place_the_stack;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.52;
+ r.begin_horizontal_move_height = 0.5;
+ r.horizontal_move_target = -0.48;
+ r.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
+ r.place_height = 0.35;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopBottomParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.move_the_stack = !place_the_stack;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.17;
+ r.begin_horizontal_move_height = 0.16;
+ r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
+ r.place_height = 0.0;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!data.GetControlBit(ControlBit::kAutonomous)) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(false)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ double intake_power = 0.0;
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ if (data.IsPressed(kRollersIn)) {
+ intake_power = 10.0;
+ claw_goal_ = 0.0;
+ }
+ if (data.IsPressed(kHorizontalCanRollersIn)) {
+ intake_power = 10.0;
+ claw_goal_ = kHorizontalCanClawAngle;
+ }
+ if (data.IsPressed(kSpit)) {
+ intake_power = -12.0;
+ action_queue_.CancelAllActions();
+ }
+
+ // Toggle button for the claw
+ if (data.PosEdge(kClawToggle)) {
+ claw_rollers_closed_ = !claw_rollers_closed_;
+ }
+
+ /*
+ if (data.IsPressed(kClawClosed)) {
+ claw_rollers_closed_ = true;
+ }
+ */
+
+ // Horizontal can pickup.
+ if (data.PosEdge(kElevatorCanUp)) {
+ actors::HorizontalCanPickupParams params;
+ params.elevator_height = 0.3;
+ params.pickup_angle = 0.80 + kHorizontalCanClawAngle;
+ params.spit_time = 0.01;
+ params.spit_power = -8.0;
+
+ params.suck_time = 0.10;
+ params.suck_power = 10.0;
+
+ params.claw_settle_time = 0.05;
+ params.claw_settle_power = 5.0;
+ params.claw_full_lift_angle = 1.35;
+ params.claw_end_angle = 0.5;
+
+ // End low so we don't drop it.
+ params.elevator_end_height = 0.28;
+ params.arm_end_angle = 0.0;
+ action_queue_.EnqueueAction(
+ actors::MakeHorizontalCanPickupAction(params));
+ fridge_closed_ = true;
+ }
+
+ // -0.942503 arm, 0.374752 elevator
+ // Vertical can pickup.
+ if (data.PosEdge(kCanPickup)) {
+ actors::CanPickupParams params;
+ params.pickup_x = 0.6;
+ params.pickup_y = 0.1;
+ params.lift_height = 0.25;
+ params.pickup_goal_before_move_height = 0.3;
+ params.start_lowering_x = 0.1;
+ // End low so the can is supported.
+ params.before_place_height = 0.4;
+ params.end_height = 0.28;
+ params.end_angle = 0.0;
+ action_queue_.EnqueueAction(actors::MakeCanPickupAction(params));
+ fridge_closed_ = true;
+ }
+
+ if (data.PosEdge(kCanReset)) {
+ action_queue_.CancelAllActions();
+ elevator_goal_ = 0.28;
+ arm_goal_ = 0.0;
+ }
+
+ // Tote chute pull in when button is pressed, pack when done.
+ if (data.IsPressed(kToteChute)) {
+ claw_goal_ = 0.8;
+ intake_power = 6.0;
+ }
+ if (data.NegEdge(kToteChute)) {
+ claw_goal_ = kClawTotePackAngle;
+ }
+
+ if (data.PosEdge(kStackAndLift)) {
+ actors::StackAndLiftParams params;
+ params.stack_params.claw_out_angle = kClawTotePackAngle;
+ params.stack_params.bottom = 0.020;
+ params.stack_params.over_box_before_place_height = 0.39;
+ params.stack_params.arm_clearance = kArmRaiseLowerClearance;
+
+ params.grab_after_stack = true;
+ params.clamp_pause_time = 0.0;
+ params.lift_params.lift_height = kStackUpHeight;
+ params.lift_params.lift_arm = kStackUpArm;
+ params.lift_params.second_lift = false;
+ params.grab_after_lift = true;
+ fridge_closed_ = true;
+
+ action_queue_.EnqueueAction(actors::MakeStackAndLiftAction(params));
+ }
+
+ if (data.PosEdge(kStackAndHold) || data.PosEdge(kSetStackDownAndHold)) {
+ actors::StackAndHoldParams params;
+ params.bottom = 0.020;
+ params.over_box_before_place_height = 0.39;
+
+ params.arm_clearance = kArmRaiseLowerClearance;
+ params.clamp_pause_time = 0.25;
+ params.claw_clamp_angle = kClawTotePackAngle;
+
+ params.hold_height = 0.68;
+ params.claw_out_angle = kClawTotePackAngle;
+
+ if (data.PosEdge(kSetStackDownAndHold)) {
+ params.place_not_stack = true;
+ params.clamp_pause_time = 0.1;
+ //params.claw_clamp_angle = kClawTotePackAngle - 0.5;
+ } else {
+ params.place_not_stack = false;
+ }
+
+ action_queue_.EnqueueAction(actors::MakeStackAndHoldAction(params));
+ fridge_closed_ = true;
+ }
+
+ // TODO(austin): Figure out what action needed to pull the 6th tote into the
+ // claw.
+
+ // Lower the fridge from holding the stack, grab the stack, and then lift.
+ if (data.PosEdge(kHeldToLift)) {
+ actors::HeldToLiftParams params;
+ params.arm_clearance = kArmRaiseLowerClearance;
+ params.clamp_pause_time = 0.1;
+ params.before_lift_settle_time = 0.1;
+ params.bottom_height = 0.020;
+ params.claw_out_angle = kClawStackClearance;
+ params.lift_params.lift_height = kStackUpHeight;
+ params.lift_params.lift_arm = kStackUpArm;
+ params.lift_params.second_lift = false;
+ fridge_closed_ = true;
+
+ action_queue_.EnqueueAction(actors::MakeHeldToLiftAction(params));
+ }
+
+ // Lift the can up.
+ if (data.PosEdge(kCanUp)) {
+ actors::LiftParams params;
+ params.lift_height = 0.68;
+ params.lift_arm = 0.3;
+ params.pack_claw = false;
+ params.pack_claw_angle = 0;
+ params.intermediate_lift_height = 0.37;
+ params.second_lift = true;
+ fridge_closed_ = true;
+
+ action_queue_.EnqueueAction(actors::MakeLiftAction(params));
+ }
+
+ // Pick up a tote from the ground and put it in the bottom of the bot.
+ if (data.PosEdge(kPickup)) {
+ actors::PickupParams params;
+ // TODO(austin): These parameters are coppied into auto right now, need
+ // to dedup...
+
+ // Lift to here initially.
+ params.pickup_angle = 0.9;
+ // Start sucking here
+ params.suck_angle = 0.8;
+ // Go back down to here to finish sucking.
+ params.suck_angle_finish = 0.4;
+ // Pack the box back in here.
+ params.pickup_finish_angle = kClawTotePackAngle;
+ params.intake_time = 0.8;
+ params.intake_voltage = 7.0;
+ action_queue_.EnqueueAction(actors::MakePickupAction(params));
+ }
+
+ // Place stack on a tote in the tray, and grab it.
+ if (data.PosEdge(kStack)) {
+ actors::StackParams params;
+ params.claw_out_angle = kClawTotePackAngle;
+ params.bottom = 0.020;
+ params.only_place = false;
+ params.arm_clearance = kArmRaiseLowerClearance;
+ params.over_box_before_place_height = 0.39;
+ action_queue_.EnqueueAction(actors::MakeStackAction(params));
+ claw_rollers_closed_ = true;
+ fridge_closed_ = true;
+ }
+
+ if (data.PosEdge(kScore)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeScoreParams()));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kCoopTop)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(false)));
+ }
+ if (data.PosEdge(kCoopTopRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(true)));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kCoopBottom)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(false)));
+ }
+ if (data.PosEdge(kCoopBottomRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(true)));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kFridgeToggle)) {
+ fridge_closed_ = !fridge_closed_;
+ }
+
+ if (data.PosEdge(ControlBit::kEnabled)) {
+ // If we got enabled, wait for everything to zero.
+ LOG(INFO, "Waiting for zero.\n");
+ waiting_for_zero_ = true;
+ }
+
+ claw_queue.status.FetchLatest();
+ fridge_queue.status.FetchLatest();
+ if (!claw_queue.status.get()) {
+ LOG(ERROR, "Got no claw status packet.\n");
+ }
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Got no fridge status packet.\n");
+ }
+
+ if (claw_queue.status.get() && fridge_queue.status.get() &&
+ claw_queue.status->zeroed && fridge_queue.status->zeroed) {
+ if (waiting_for_zero_) {
+ LOG(INFO, "Zeroed! Starting teleop mode.\n");
+ waiting_for_zero_ = false;
+
+ // Set the initial goals to where we are now.
+ elevator_goal_ = 0.3;
+ arm_goal_ = 0.0;
+ claw_goal_ = 0.6;
+ }
+ } else {
+ waiting_for_zero_ = true;
+ }
+
+ if (!waiting_for_zero_) {
+ if (!action_queue_.Running()) {
+ auto new_fridge_goal = fridge_queue.goal.MakeMessage();
+ new_fridge_goal->max_velocity = elevator_params_.velocity;
+ new_fridge_goal->max_acceleration = elevator_params_.acceleration;
+ new_fridge_goal->profiling_type = 0;
+ new_fridge_goal->height = elevator_goal_;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_params_.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_params_.acceleration;
+ new_fridge_goal->angle = arm_goal_;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = fridge_closed_;
+ new_fridge_goal->grabbers.top_back = fridge_closed_;
+ new_fridge_goal->grabbers.bottom_front = fridge_closed_;
+ new_fridge_goal->grabbers.bottom_back = fridge_closed_;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Sending fridge goal failed.\n");
+ } else {
+ LOG(DEBUG, "sending goals: elevator: %f, arm: %f\n", elevator_goal_,
+ arm_goal_);
+ }
+ if (!claw_queue.goal.MakeWithBuilder()
+ .angle(claw_goal_)
+ .rollers_closed(claw_rollers_closed_)
+ .max_velocity(5.0)
+ .max_acceleration(6.0)
+ .intake(intake_power)
+ .Send()) {
+ LOG(ERROR, "Sending claw goal failed.\n");
+ }
+ }
+ }
+
+ if (action_queue_.Running()) {
+ // If we are running an action, update our goals to the current goals.
+ control_loops::fridge_queue.status.FetchLatest();
+ if (control_loops::fridge_queue.status.get()) {
+ arm_goal_ = control_loops::fridge_queue.status->goal_angle;
+ elevator_goal_ = control_loops::fridge_queue.status->goal_height;
+ } else {
+ LOG(ERROR, "No fridge status!\n");
+ }
+
+ // If we are running an action, update our goals to the current goals.
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ claw_goal_ = control_loops::claw_queue.status->goal_angle;
+ } else {
+ LOG(ERROR, "No claw status!\n");
+ }
+ }
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+ }
+
+ private:
+ void StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
+ }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+ }
+
+ bool was_running_;
+
+ // Previous goals for systems.
+ double elevator_goal_ = 0.2;
+ double arm_goal_ = 0.0;
+ double claw_goal_ = 0.0;
+ bool claw_rollers_closed_ = false;
+ bool fridge_closed_ = false;
+ actors::ProfileParams arm_params_ = kArmMove;
+ actors::ProfileParams elevator_params_ = kElevatorMove;
+
+ // If we're waiting for the subsystems to zero.
+ bool waiting_for_zero_ = true;
+
+ bool auto_running_ = false;
+
+ ::aos::common::actions::ActionQueue action_queue_;
+
+ ::aos::util::SimpleLogInterval no_drivetrain_status_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ "no drivetrain status");
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}