diff --git a/PROJECT-FUZZY-WUZZY/include/globals.hpp b/PROJECT-FUZZY-WUZZY/include/globals.hpp index d8bcf05..09c757b 100644 --- a/PROJECT-FUZZY-WUZZY/include/globals.hpp +++ b/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; } \ No newline at end of file diff --git a/PROJECT-FUZZY-WUZZY/src/globals.cpp b/PROJECT-FUZZY-WUZZY/src/globals.cpp index 73c25dc..719237a 100644 --- a/PROJECT-FUZZY-WUZZY/src/globals.cpp +++ b/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 + ); + } \ No newline at end of file