y2016_bot3/joystick_reader.cc

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>

#include "aos/linux_code/init.h"
#include "aos/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 "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016_bot3/control_loops/intake/intake.q.h"
#include "y2016_bot3/control_loops/intake/intake.h"
#include "y2016_bot3/queues/ball_detector.q.h"

#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
#include "y2016_bot3/actors/autonomous_actor.h"
#include "y2016_bot3/control_loops/drivetrain/drivetrain_base.h"

using ::frc971::control_loops::drivetrain_queue;
using ::y2016_bot3::control_loops::intake_queue;

using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;

namespace y2016_bot3 {
namespace input {
namespace joysticks {

const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kQuickTurn(1, 5);

const ButtonLocation kTurn1(1, 7);
const ButtonLocation kTurn2(1, 11);

// Buttons on the lexan driver station to get things running on bring-up day.
const ButtonLocation kIntakeDown(3, 5);
const ButtonLocation kIntakeIn(3, 4);
const ButtonLocation kFire(3, 3);
const ButtonLocation kIntakeOut(3, 3);
const ButtonLocation kChevalDeFrise(3, 11);

class Reader : public ::aos::input::JoystickInput {
 public:
  Reader()
      : intake_goal_(0.0),
        dt_config_(control_loops::drivetrain::GetDrivetrainConfig()) {}

  void RunIteration(const ::aos::input::driver_station::Data &data) override {
    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::kEnabled)) {
      // If we are not enabled, reset the waiting for zero bit.
      LOG(DEBUG, "Waiting for zero.\n");
      waiting_for_zero_ = true;
    }

    if (!auto_running_) {
      HandleDrivetrain(data);
      HandleTeleop(data);
    }

    // Process any pending actions.
    action_queue_.Tick();
    was_running_ = action_queue_.Running();
  }

  void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
    bool is_control_loop_driving = false;
    static double left_goal = 0.0;
    static double right_goal = 0.0;

    const double wheel = -data.GetAxis(kSteeringWheel);
    const double throttle = -data.GetAxis(kDriveThrottle);
    drivetrain_queue.status.FetchLatest();

    if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
      if (drivetrain_queue.status.get()) {
        left_goal = drivetrain_queue.status->estimated_left_position;
        right_goal = drivetrain_queue.status->estimated_right_position;
      }
    }
    if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
      is_control_loop_driving = true;
    }
    if (!drivetrain_queue.goal.MakeWithBuilder()
             .steering(wheel)
             .throttle(throttle)
             .quickturn(data.IsPressed(kQuickTurn))
             .control_loop_driving(is_control_loop_driving)
             .left_goal(left_goal - wheel * 0.5 + throttle * 0.3)
             .right_goal(right_goal + wheel * 0.5 + throttle * 0.3)
             .left_velocity_goal(0)
             .right_velocity_goal(0)
             .Send()) {
      LOG(WARNING, "sending stick values failed\n");
    }
  }

  void HandleTeleop(const ::aos::input::driver_station::Data &data) {
    intake_goal_ = y2016_bot3::constants::kIntakeRange.upper - 0.04;

    if (!data.GetControlBit(ControlBit::kEnabled)) {
      action_queue_.CancelAllActions();
      LOG(DEBUG, "Canceling\n");
    }

    intake_queue.status.FetchLatest();
    if (!intake_queue.status.get()) {
      LOG(ERROR, "Got no intake status packet.\n");
    }

    if (intake_queue.status.get() && intake_queue.status->zeroed) {
      if (waiting_for_zero_) {
        LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
        waiting_for_zero_ = false;
      }
    } else {
      waiting_for_zero_ = true;
    }

    bool ball_detected = false;
    ::y2016_bot3::sensors::ball_detector.FetchLatest();
    if (::y2016_bot3::sensors::ball_detector.get()) {
      ball_detected = ::y2016_bot3::sensors::ball_detector->voltage > 2.5;
    }
    if (data.PosEdge(kIntakeIn)) {
      saw_ball_when_started_intaking_ = ball_detected;
    }

    is_intaking_ = data.IsPressed(kIntakeIn) &&
                   (!ball_detected || saw_ball_when_started_intaking_);

    is_outtaking_ = data.IsPressed(kIntakeOut);

    if (is_intaking_ || is_outtaking_) {
      recently_intaking_accumulator_ = 20;
    }

    if (data.IsPressed(kIntakeDown)) {
      if (recently_intaking_accumulator_) {
        intake_goal_ = 0.1;
      } else {
        intake_goal_ = -0.05;
      }
    }

    if (recently_intaking_accumulator_ > 0) {
      --recently_intaking_accumulator_;
    }

    if (data.IsPressed(kChevalDeFrise)) {
      traverse_down_ = true;
    } else {
      traverse_down_ = false;
    }

    if (!waiting_for_zero_) {
      auto new_intake_goal = intake_queue.goal.MakeMessage();
      new_intake_goal->angle_intake = intake_goal_;

      new_intake_goal->max_angular_velocity_intake = 7.0;
      new_intake_goal->max_angular_acceleration_intake = 40.0;

      static constexpr bool kGrannyMode = false;
      if (kGrannyMode) {
        new_intake_goal->max_angular_velocity_intake = 0.2;
        new_intake_goal->max_angular_acceleration_intake = 1.0;
      }

      if (is_intaking_) {
        new_intake_goal->voltage_intake_rollers = -12.0;
        new_intake_goal->voltage_top_rollers = -12.0;
        new_intake_goal->voltage_bottom_rollers = 12.0;
      } else if (is_outtaking_) {
        new_intake_goal->voltage_intake_rollers = 12.0;
        new_intake_goal->voltage_top_rollers = 12.0;
        new_intake_goal->voltage_bottom_rollers = -7.0;
      } else {
        new_intake_goal->voltage_intake_rollers = 0.0;
        new_intake_goal->voltage_top_rollers = 0.0;
        new_intake_goal->voltage_bottom_rollers = 0.0;
      }

      new_intake_goal->traverse_down = traverse_down_;
      new_intake_goal->force_intake = true;

      if (!new_intake_goal.Send()) {
        LOG(ERROR, "Sending intake goal failed.\n");
      } else {
        LOG(DEBUG, "sending goal: intake: %f\n", intake_goal_);
      }
    }
  }

 private:
  void StartAuto() {
    LOG(INFO, "Starting auto mode\n");

    actors::AutonomousActionParams params;
    actors::auto_mode.FetchLatest();
    if (actors::auto_mode.get() != nullptr) {
      params.mode = actors::auto_mode->mode;
    } else {
      LOG(WARNING, "no auto mode values\n");
      params.mode = 0;
    }
    action_queue_.EnqueueAction(actors::MakeAutonomousAction(params));
  }

  void StopAuto() {
    LOG(INFO, "Stopping auto mode\n");
    action_queue_.CancelAllActions();
  }

  // Whatever these are set to are our default goals to send out after zeroing.
  double intake_goal_;

  bool was_running_ = false;
  bool auto_running_ = false;

  bool traverse_down_ = false;

  // If we're waiting for the subsystems to zero.
  bool waiting_for_zero_ = true;

  // If true, the ball was present when the intaking button was pressed.
  bool saw_ball_when_started_intaking_ = false;

  bool is_intaking_ = false;
  bool is_outtaking_ = false;

  int recently_intaking_accumulator_ = 0;

  ::aos::common::actions::ActionQueue action_queue_;

  const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;

  ::aos::util::SimpleLogInterval no_drivetrain_status_ =
      ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
                                     "no drivetrain status");
};

}  // namespace joysticks
}  // namespace input
}  // namespace y2016_bot3

int main() {
  ::aos::Init(-1);
  ::y2016_bot3::input::joysticks::Reader reader;
  reader.Run();
  ::aos::Cleanup();
}