mecanum_drive.h

#pragma once
 
#include "../core/include/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;
};