drive_commands.h

 
#pragma once
 
#include "core/subsystems/tank_drive.h"
#include "core/utils/command_structure/auto_command.h"
#include "core/utils/geometry.h"
#include "vex.h"
#include "core/utils/math/geometry/pose2d.h"
using namespace vex;
 
// ==== DRIVING ====

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

  bool run() override;
 
  /*
  * Returns a string describing the commands functionality
  */
  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;
  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;
 
  /*
  * Returns a string describing the commands functionality
  */
  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, directionType dir,
                      double max_speed = 1, double end_speed = 0);
  DriveToPointCommand(TankDrive &drive_sys, Feedback &feedback, Translation2d translation, directionType dir, double max_speed = 1,
                      double end_speed = 0);

  bool run() override;
 
  /*
  * Returns a string describing the commands functionality
  */
  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;
  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;
 
    /*
    * Returns a string describing the commands functionality
    */
    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;
 
  /*
  * Returns a string describing the commands functionality
  */
  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 PurePursuitCommand : public AutoCommand {
public:
  PurePursuitCommand(TankDrive &drive_sys, Feedback &feedback, PurePursuit::Path path, directionType dir,
                     double max_speed = 1, double end_speed = 0);

  bool run() override;
 
  /*
  * Returns a string describing the commands functionality
  */
  std::string toString() override;

  void on_timeout() override;
 
private:
  TankDrive &drive_sys;
  PurePursuit::Path path;
  directionType dir;
  Feedback &feedback;
  double max_speed;
  double end_speed;
};

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

  bool run() override;
 
  /*
  * Returns a string describing the commands functionality
  */
  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;
  /*
  * Returns a string describing the commands functionality
  */
  std::string toString() override;
private:
  // drive system with an odometry config
  OdometryBase &odom;
  Pose2d newpos;
};