tank_drive.h

#pragma once
 
#ifndef PI
#define PI 3.141592654
#endif
 
#include "../core/include/robot_specs.h"
#include "../core/include/subsystems/odometry/odometry_tank.h"
#include "../core/include/utils/command_structure/auto_command.h"
#include "../core/include/utils/controls/feedback_base.h"
#include "../core/include/utils/controls/pid.h"
#include "../core/include/utils/pure_pursuit.h"
#include "vex.h"
#include <vector>
 
using namespace vex;
 
class TankDrive {
public:
  enum class BrakeType {
    None,         
    ZeroVelocity, 
    Smart,        
  };
  TankDrive(motor_group &left_motors, motor_group &right_motors, robot_specs_t &config, OdometryBase *odom = NULL);
 
  AutoCommand *DriveToPointCmd(point_t pt, vex::directionType dir = vex::forward, double max_speed = 1.0,
                               double end_speed = 0.0);
  AutoCommand *DriveToPointCmd(Feedback &fb, point_t pt, 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 *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();
 
  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);
 
  bool pure_pursuit(PurePursuit::Path path, directionType dir, double max_speed = 1, double end_speed = 0);
 
private:
  motor_group &left_motors;  
  motor_group &right_motors; 
 
  PID correction_pid;                      
  Feedback *drive_default_feedback = NULL; 
  Feedback *turn_default_feedback = NULL;  
 
  OdometryBase *odometry; 
 
  robot_specs_t
      &config; 
 
  bool func_initialized = false; 
  bool is_pure_pursuit = false;  
};