tank_drive.h

#pragma once
 
#ifndef PI
#define PI 3.141592654
#endif
 
#include "core/robot_specs.h"
#include "core/subsystems/odometry/odometry_tank.h"
#include "core/utils/command_structure/auto_command.h"
#include "core/utils/controls/feedback_base.h"
#include "core/utils/controls/pid.h"
#include "core/utils/pure_pursuit.h"
#include "vex.h"
#include <vector>
 
using namespace vex;

class TankDrive {
  public:
    enum class BrakeType {
        None,         
        ZeroVelocity, 
        Smart,        
        TurnOnly,
    };

    TankDrive(motor_group &left_motors, motor_group &right_motors, robot_specs_t &config, OdometryBase *odom = NULL);
 
    AutoCommand *DriveToPointCmd(
      Translation2d pt, vex::directionType dir = vex::forward, double max_speed = 1.0, double end_speed = 0.0
    );
    AutoCommand *DriveToPointCmd(
      Feedback &fb, Translation2d pt, vex::directionType dir = vex::forward, double max_speed = 1.0,
      double end_speed = 0.0
    );
 
    AutoCommand *DriveToPointCmd(
      double x, double y, vex::directionType dir = vex::forward, double max_speed = 1.0, double end_speed = 0.0
    );
 
    AutoCommand *
    DriveForwardCmd(double dist, vex::directionType dir = vex::forward, double max_speed = 1.0, double end_speed = 0.0);
    AutoCommand *DriveForwardCmd(
      Feedback &fb, double dist, vex::directionType dir = vex::forward, double max_speed = 1.0, double end_speed = 0.0
    );
 
    AutoCommand *TurnToHeadingCmd(double heading, double max_speed = 1.0, double end_speed = 0.0);
    AutoCommand *TurnToHeadingCmd(Feedback &fb, double heading, double max_speed = 1.0, double end_speed = 0.0);
 
    AutoCommand *TurnToPointCmd(
      double x, double y, vex::directionType dir = vex::directionType::fwd, double max_speed = 1.0,
      double end_speed = 0.0
    );
    AutoCommand *TurnToPointCmd(
      Translation2d point, vex::directionType dir = vex::directionType::fwd, double max_speed = 1.0,
      double end_speed = 0.0
    );
 
    AutoCommand *TurnDegreesCmd(double degrees, double max_speed = 1.0, double start_speed = 0.0);
    AutoCommand *TurnDegreesCmd(Feedback &fb, double degrees, double max_speed = 1.0, double end_speed = 0.0);
 
    AutoCommand *PurePursuitCmd(PurePursuit::Path path, directionType dir, double max_speed = 1, double end_speed = 0);
    AutoCommand *PurePursuitCmd(
      Feedback &feedback, PurePursuit::Path path, directionType dir, double max_speed = 1, double end_speed = 0
    );
    Condition *DriveStalledCondition(double stall_time);
    AutoCommand *DriveTankCmd(double left, double right);

    void stop();

    Pose2d get_position();

    void drive_tank(double left, double right, int power = 1, BrakeType bt = BrakeType::None);
    void drive_tank_raw(double left, double right);

    void drive_arcade(double forward_back, double left_right, int power = 1, BrakeType bt = BrakeType::None);

    bool
    drive_forward(double inches, directionType dir, Feedback &feedback, double max_speed = 1, double end_speed = 0);

    bool drive_forward(double inches, directionType dir, double max_speed = 1, double end_speed = 0);

    bool turn_degrees(double degrees, Feedback &feedback, double max_speed = 1, double end_speed = 0);

    bool turn_degrees(double degrees, double max_speed = 1, double end_speed = 0);

    bool drive_to_point(
      double x, double y, vex::directionType dir, Feedback &feedback, double max_speed = 1, double end_speed = 0
    );

    bool drive_to_point(double x, double y, vex::directionType dir, double max_speed = 1, double end_speed = 0);

    bool turn_to_heading(double heading_deg, Feedback &feedback, double max_speed = 1, double end_speed = 0);
    bool turn_to_heading(double heading_deg, double max_speed = 1, double end_speed = 0);

    void reset_auto();

    static double modify_inputs(double input, int power = 2);

    bool pure_pursuit(
      PurePursuit::Path path, directionType dir, Feedback &feedback, double max_speed = 1, double end_speed = 0
    );

  private:
    bool pure_pursuit(PurePursuit::Path path, directionType dir, double max_speed = 1, double end_speed = 0);
    motor_group &left_motors;  
    motor_group &right_motors; 
 
    OdometryBase *odometry; 
 
    PID correction_pid;                      
    Feedback *drive_default_feedback = NULL; 
    Feedback *turn_default_feedback = NULL;  
 
    robot_specs_t
      &config; 
 
    bool func_initialized = false; 
    bool is_pure_pursuit = false;  
};