mecanum_drive.h

#pragma once
 
#include "core/utils/controls/pid.h"
#include "vex.h"
 
#ifndef PI
#define PI 3.141592654
#endif

class MecanumDrive {
 
  public:
    struct mecanumdrive_config_t {
        // PID configurations for autonomous driving
        PID::pid_config_t drive_pid_conf;
        PID::pid_config_t drive_gyro_pid_conf;
        PID::pid_config_t turn_pid_conf;
 
        // Diameter of the mecanum wheels
        double drive_wheel_diam;
 
        // Diameter of the perpendicular undriven encoder wheel
        double lateral_wheel_diam;
 
        // Width between the center of the left and right wheels
        double wheelbase_width;
    };

    MecanumDrive(
      vex::motor &left_front, vex::motor &right_front, vex::motor &left_rear, vex::motor &right_rear,
      vex::rotation *lateral_wheel = NULL, vex::inertial *imu = NULL, mecanumdrive_config_t *config = NULL
    );

    void drive_raw(double direction_deg, double magnitude, double rotation);

    void drive(double left_y, double left_x, double right_x, int power = 2);

    bool auto_drive(double inches, double direction, double speed, bool gyro_correction = true);

    bool auto_turn(double degrees, double speed, bool ignore_imu = false);
 
  private:
    vex::motor &left_front, &right_front, &left_rear, &right_rear;
 
    mecanumdrive_config_t *config;
    vex::rotation *lateral_wheel;
    vex::inertial *imu;
 
    PID *drive_pid = NULL;
    PID *drive_gyro_pid = NULL;
    PID *turn_pid = NULL;
 
    bool init = true;
};