drive_commands.h

 
#pragma once
 
#include "../core/include/subsystems/tank_drive.h"
#include "../core/include/utils/command_structure/auto_command.h"
#include "../core/include/utils/geometry.h"
#include "vex.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;
  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;
  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, point_t point, directionType dir, double max_speed = 1,
                      double end_speed = 0);
 
  bool run() 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 TurnToHeadingCommand : public AutoCommand {
public:
  TurnToHeadingCommand(TankDrive &drive_sys, Feedback &feedback, double heading_deg, double speed = 1,
                       double end_speed = 0);
 
  bool run() 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;
 
  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;
  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 pose_t &newpos = OdometryBase::zero_pos);
 
  bool run() override;
 
private:
  // drive system with an odometry config
  OdometryBase &odom;
  pose_t newpos;
};