tank_drive.h

#pragma once
 
#ifndef PI
#define PI 3.141592654
#endif
 
#include "vex/vex.h"
 
#include "core/robot_specs.h"
#include "core/subsystems/odometry/odometry_tank.h"
#include "core/competition/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 <vector>

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

    TankDrive(
            vex::motor_group& left_motors, vex::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, vex::directionType dir, double max_speed = 1,
            double end_speed = 0
    );
    AutoCommand* PurePursuitCmd(
            Feedback& feedback, PurePursuit::Path path, vex::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, vex::directionType dir, Feedback& feedback, double max_speed = 1,
            double end_speed = 0
    );

    bool drive_forward(
            double inches, vex::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, vex::directionType dir, Feedback& feedback,
            double max_speed = 1, double end_speed = 0
    );

   private:
    bool pure_pursuit(
            PurePursuit::Path path, vex::directionType dir, double max_speed = 1,
            double end_speed = 0
    );
    vex::motor_group& left_motors;   
    vex::motor_group& right_motors;  
 
    OdometryBase* odometry;  
 
    Feedback* 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;  
};