global work

PROJECT-FUZZY-WUZZY/include/globals.hpp

@@ -1,8 +1,17 @@
 #pragma once
 
+// CONSTANTS DEFINITIONS
+#define HORIZONTAL_TW_OFFSET 1.0
+#define VERTICAL_TW_OFFSET 1.0
+#define CHASSIS_TRACK_WIDTH 12.5
+#define CHASSIS_RPM 450
+#define CHASE_POWER 2
+
 #include "main.h"
 
 namespace globals {
+    extern pros::Controller master;
+
     extern pros::Motor left_front;
     extern pros::Motor left_middle;
     extern pros::Motor left_back;
@@ -14,4 +23,19 @@ namespace globals {
     extern pros::Motor_Group right_drive;
     
     extern pros::Imu imu;
+
+    // ODOM. SENSORS
+    extern pros::Rotation horizontal_tw_enc;
+    extern pros::Rotation vertical_tw_enc;
+
+    // LEM IMPL.
+    extern lemlib::TrackingWheel horizontal_tw;
+    extern lemlib::TrackingWheel vertical_tw;
+
+    extern lemlib::Drivetrain drivetrain;
+    extern lemlib::ControllerSettings lateral_controller;
+    extern lemlib::ControllerSettings angular_controller;
+
+    extern lemlib::OdomSensors odom_sensors;
+    extern lemlib::Chassis chassis;
 }

PROJECT-FUZZY-WUZZY/src/globals.cpp

@@ -1,7 +1,11 @@
 #include "main.h"
 #include "globals.hpp"
 
+using namespace lemlib;
+
 namespace globals {
+    pros::Controller master(pros::E_CONTROLLER_MASTER);
+
     pros::Motor left_front(1, pros::E_MOTOR_GEARSET_06, false);
     pros::Motor left_middle(2, pros::E_MOTOR_GEARSET_06, false);
     pros::Motor left_back(3, pros::E_MOTOR_GEARSET_06, false);
@@ -13,4 +17,61 @@ namespace globals {
     pros::Motor_Group right_drive({ right_front, right_middle, right_back });
     
     pros::Imu imu(7);
+
+    //ODOM. SENSORS
+    pros::Rotation horizontal_tw_enc(8, false);
+    pros::Rotation vertical_tw_enc(9, false);
+
+    //LEM IMPL.
+    TrackingWheel horizontal_tw(&horizontal_tw_enc, Omniwheel::NEW_275, HORIZONTAL_TW_OFFSET);
+    TrackingWheel vertical_tw(&vertical_tw_enc, Omniwheel::NEW_275, VERTICAL_TW_OFFSET);
+
+    Drivetrain drivetrain(
+        &left_drive,
+        &right_drive,
+        CHASSIS_TRACK_WIDTH,
+        Omniwheel::NEW_325,
+        CHASSIS_RPM,
+        CHASE_POWER
+    );
+
+    ControllerSettings lateral_controller(
+        10, // proportional gain (kP)
+        0, // integral gain (kI)
+        3, // derivative gain (kD)
+        3, // anti windup
+        1, // small error range, in inches
+        100, // small error range timeout, in milliseconds
+        3, // large error range, in inches
+        500, // large error range timeout, in milliseconds
+        20 // maximum acceleration (slew)
+    );
+
+    ControllerSettings angular_controller(
+        2, // proportional gain (kP)
+        0, // integral gain (kI)
+        10, // derivative gain (kD)
+        3, // anti windup
+        1, // small error range, in degrees
+        100, // small error range timeout, in milliseconds
+        3, // large error range, in degrees
+        500, // large error range timeout, in milliseconds
+        0 // maximum acceleration (slew)
+    );
+
+    OdomSensors odom_sensors(
+        &vertical_tw,
+        nullptr,
+        &horizontal_tw,
+        nullptr,
+        &imu
+    );
+
+    Chassis chassis(
+        drivetrain,
+        lateral_controller,
+        angular_controller,
+        odom_sensors
+    );
+
 }