main.cpp

Go to the documentation of this file.

#include "main.h"
#include "lemlib/api.hpp" // IWYU pragma: keep
#include "lemlib/chassis/chassis.hpp"
#include "lemlib/chassis/trackingWheel.hpp"
#include "liblvgl/llemu.hpp"
#include "pros/misc.h"
 
 
pros::Controller controller(CONTROLLER_MASTER);
 
pros::MotorGroup rightMotors({-11,2,1});
pros::MotorGroup leftMotors({-16,14,-13});
 
lemlib::Drivetrain drivetrain(&leftMotors, &rightMotors, 10, 3.25, 450, 2);
 
pros::IMU imu(3);
 
pros::Rotation horizontal_tracking_sensor(12);
pros::Rotation vertical_tracking_sensor(15);
 
lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_tracking_sensor, 3.75, 9/2.54, 1);
lemlib::TrackingWheel vertical_tracking_wheel(&vertical_tracking_sensor, 3.75, 7/100/2.54,1);
lemlib::OdomSensors sensors(&vertical_tracking_wheel, nullptr, &horizontal_tracking_wheel, nullptr, &imu);
 
// lateral PID controller
lemlib::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)
);
 
// angular PID controller
lemlib::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)
);
 
lemlib::Chassis chassis(drivetrain, lateral_controller, angular_controller, sensors);
 

void initialize() {
    pros::lcd::initialize(); // initialize brain screen
    chassis.calibrate(); // calibrate sensors
    // print position to brain screen
    pros::Task screen_task([&]() {
        while (true) {
            // print robot location to the brain screen
            pros::lcd::print(0, "X: %f", chassis.getPose().x); // x
            pros::lcd::print(1, "Y: %f", chassis.getPose().y); // y
            pros::lcd::print(2, "Theta: %f", chassis.getPose().theta); // heading
            // delay to save resources
            pros::delay(20);
        }
    });
}

void disabled() {}

void competition_initialize() {}

void autonomous() {}

 
 
 
void opcontrol() {
    while (true)
    {
        int rightControl = controller.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y);
        int leftControl = controller.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y);
 
        chassis.tank(leftControl, rightControl, false);
 
        pros::delay(20);
    }
}