drive_commands.h

 
#pragma once
 
#include "core/competition/command_structure/auto_command.h"
#include "core/subsystems/tank_drive.h"
#include "core/math/geometry/pose2d.h"
#include "vex/vex.h"
 
// ==== DRIVING ====

class DriveForwardCommand : public AutoCommand {
 public:
  DriveForwardCommand(
      TankDrive& drive_sys, Feedback& feedback, double inches, vex::directionType dir,
      double max_speed = 1, double end_speed = 0
  );

  bool run() override;

  std::string toString() override;

  void on_timeout() override;
 
 private:
  // drive system to run the function on
  TankDrive& drive_sys;
 
  // feedback controller to use
  Feedback& feedback;
 
  // parameters for drive_forward
  double inches;
  vex::directionType dir;
  double max_speed;
  double end_speed;
};

class TurnDegreesCommand : public AutoCommand {
 public:
  TurnDegreesCommand(
      TankDrive& drive_sys, Feedback& feedback, double degrees, double max_speed = 1,
      double end_speed = 0
  );

  bool run() override;

  std::string toString() override;

  void on_timeout() override;
 
 private:
  // drive system to run the function on
  TankDrive& drive_sys;
 
  // feedback controller to use
  Feedback& feedback;
 
  // parameters for turn_degrees
  double degrees;
  double max_speed;
  double end_speed;
};

class DriveToPointCommand : public AutoCommand {
 public:
  DriveToPointCommand(
      TankDrive& drive_sys, Feedback& feedback, double x, double y, vex::directionType dir,
      double max_speed = 1, double end_speed = 0
  );
  DriveToPointCommand(
      TankDrive& drive_sys, Feedback& feedback, Translation2d translation, vex::directionType dir,
      double max_speed = 1, double end_speed = 0
  );

  bool run() override;

  std::string toString() override;
 
 private:
  // drive system to run the function on
  TankDrive& drive_sys;

  void on_timeout() override;
 
  // feedback controller to use
  Feedback& feedback;
 
  // parameters for drive_to_point
  double x;
  double y;
  vex::directionType dir;
  double max_speed;
  double end_speed;
};
 
class TurnToPointCommand : public AutoCommand {
 public:
  TurnToPointCommand(
      TankDrive& drive_sys, double x, double y, vex::directionType dir, double max_speed = 1,
      double end_speed = 0
  );
  TurnToPointCommand(
      TankDrive& drive_sys, Translation2d translation, vex::directionType dir, double max_speed = 1,
      double end_speed = 0
  );
 
  bool run() override;

  std::string toString() override;
 
 private:
  void on_timeout() override;
  TankDrive& drive_sys;
  double x, y;
  vex::directionType dir;
  double max_speed;
  double end_speed;
  bool func_initialized = false;
  double heading;
};

class TurnToHeadingCommand : public AutoCommand {
 public:
  TurnToHeadingCommand(
      TankDrive& drive_sys, Feedback& feedback, double heading_deg, double speed = 1,
      double end_speed = 0
  );

  bool run() override;

  std::string toString() override;

  void on_timeout() override;
 
 private:
  // drive system to run the function on
  TankDrive& drive_sys;
 
  // feedback controller to use
  Feedback& feedback;
 
  // parameters for turn_to_heading
  double heading_deg;
  double max_speed;
  double end_speed;
};

class DriveStopCommand : public AutoCommand {
 public:
  DriveStopCommand(TankDrive& drive_sys);

  bool run() override;

  std::string toString() override;
 
  void on_timeout() override;
 
 private:
  // drive system to run the function on
  TankDrive& drive_sys;
};
 
// ==== ODOMETRY ====

class OdomSetPosition : public AutoCommand {
 public:
  OdomSetPosition(OdometryBase& odom, const Pose2d& newpos = OdometryBase::zero_pos);

  bool run() override;

  std::string toString() override;
 
 private:
  // drive system with an odometry config
  OdometryBase& odom;
  Pose2d newpos;
};