Over the summer I made a robot code meant to control a 6 motor drivetrain, but there is no response from the robot whatsoever. All the motor ports are correct, here is the code:
#include "main.h"
#include "lemlib/chassis/chassis.hpp"
#include "pros/abstract_motor.hpp"
#include "pros/misc.h"
#include "lemlib/api.hpp" // IWYU pragma: keep
#include <sys/\_intsup.h>
pros::MotorGroup left_mg({8,6,11 }, pros::MotorGearset::blue); // Creates a motor group with reversed port 1
pros::MotorGroup right_mg({-7,-2,-3 }, pros::MotorGearset::blue); // Creates a motor group with forwards port 10
pros::Controller master(pros::E_CONTROLLER_MASTER);
// drivetrain settings
lemlib::Drivetrain drivetrain(&left_mg, // left motor group
&right_mg, // right motor group
10, // 11.75 inch track width
2.75, // using new 4" omnis see descriptions on lemlib documentation
450, // drivetrain rpm is 450
2 // horizontal drift is 2 but will be tuned to higher numbers if the drivetrain is less suseptable to sliding.
);
lemlib::ExpoDriveCurve throttle_curve(
3,
10,
1.019
);
// create an imu on port 9
pros::Imu imu(9); // change this number to the port the inertial sensor is in.
// horizontal tracking wheel encoder
pros::Rotation horizontal_encoder(20);
// vertical tracking wheel encoder
pros::Rotation vertical_encoder(19); //change these ports to the correct ones for your robot, these are just placeholders
// horizontal tracking wheel
lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_encoder, lemlib::Omniwheel::NEW_275, -5.75);
// vertical tracking wheel
lemlib::TrackingWheel vertical_tracking_wheel(&vertical_encoder, lemlib::Omniwheel::NEW_275, -2.5); // these distances are for the offset from the center of your robot.
lemlib::OdomSensors sensors(nullptr, // vertical tracking wheel 1, set to nullptr until we use one
nullptr, // vertical tracking wheel 2, set to nullptr as we are using IMEs
nullptr, // horizontal tracking wheel 1 set to nullptr until we use one
nullptr, // horizontal tracking wheel 2, set to nullptr as we don't have a second one
&imu // inertial sensor
);
// lateral PID controller
lemlib::ControllerSettings lateral_controller(10, // proportional gain (kP)
0, // integral gain (kI) Tune these by having the robot going 24 inches, and using the flowchart on lemlib documentation.
3, // derivative gain (kD) these you will use to tune, but the ones below should be set to 0 when tuning and to default during autonomous.
3, // anti windup See lemlib documentation for default settings
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) // Tune these by having the robot turn 90º and using the flowchart on lemlib documentation.
3, // anti windup See lemlib documentation for default settings
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)
);
// create the chassis
lemlib::Chassis chassis(drivetrain, // drivetrain settings
lateral_controller, // lateral PID settings
angular_controller, // angular PID settings
sensors // odometry sensors
);
/\*\*
\* A callback function for LLEMU's center button.
\*
\* When this callback is fired, it will toggle line 2 of the LCD text between
\* "I was pressed!" and nothing.
\*/
void on_center_button() {
static bool pressed = false;
pressed = !pressed;
if (pressed) {
pros::lcd::set_text(2, "I was pressed!");
} else {
pros::lcd::clear_line(2); // these are as a test and do not need to be removed.
}
}
/\*\*
\* Runs initialization code. This occurs as soon as the program is started.
\*
\* All other competition modes are blocked by initialize; it is recommended
\* to keep execution time for this mode under a few seconds.
\*/
void initialize() {
pros::lcd::initialize();
pros::lcd::set_text(1, "Initialized");
pros::lcd::register_btn1_cb(on_center_button);
chassis.calibrate(); // calibrate sensors
}
/\*\*
\* Runs while the robot is in the disabled state of Field Management System or
\* the VEX Competition Switch, following either autonomous or opcontrol. When
\* the robot is enabled, this task will exit.
\*/
void disabled() {}
/\*\*
\* Runs after initialize(), and before autonomous when connected to the Field
\* Management System or the VEX Competition Switch. This is intended for
\* competition-specific initialization routines, such as an autonomous selector
\* on the LCD.
\*
\* This task will exit when the robot is enabled and autonomous or opcontrol
\* starts.
\*/
void competition_initialize() {}
/\*\*
\* Runs the user autonomous code. This function will be started in its own task
\* with the default priority and stack size whenever the robot is enabled via
\* the Field Management System or the VEX Competition Switch in the autonomous
\* mode. Alternatively, this function may be called in initialize or opcontrol
\* for non-competition testing purposes.
\*
\* If the robot is disabled or communications is lost, the autonomous task
\* will be stopped. Re-enabling the robot will restart the task, not re-start it
\* from where it left off.
\*/
void autonomous() {
// set position to x:0, y:0, heading:0
chassis.setPose(72, 0, 0); // change this value to the starting position of your robot, these are just placeholders.
// turn to face heading 90 with a very long timeout
chassis.moveToPose(
0, // x = 0 switch these to the position you want to move to, these are just placeholders
0, // y = 0
0, // theta = 0 Theta is the heading of the robot, and changing it will turn the robot.
4000, // timeout of 4000ms
{.horizontalDrift = 2, .lead = 0.3}
);
}
/\*\*
\* Runs the operator control code. This function will be started in its own task
\* with the default priority and stack size whenever the robot is enabled via
\* the Field Management System or the VEX Competition Switch in the operator
\* control mode.
\*
\* If no competition control is connected, this function will run immediately
\* following initialize().
\*
\* If the robot is disabled or communications is lost, the
\* operator control task will be stopped. Re-enabling the robot will restart the
\* task, not resume it from where it left off.
\*/
void opcontrol() {
//pros::Motor intake_motor(20);
//pros::Motor extra_motor(20); // these are extra motors, these are just used for testing C++.
pros::lcd::print(0, "%d %d %d", (pros::lcd::read_buttons() & LCD_BTN_LEFT) >> 2,
(pros::lcd::read_buttons() & LCD_BTN_CENTER) >> 1,
(pros::lcd::read_buttons() & LCD_BTN_RIGHT) >> 0); // Prints status of the emulated screen LCDs
// Arcade control scheme
int lefty = master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y); // Gets amount forward/backward from left joystick
int rightx = master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X); // Gets the turn left/right from right joystick // move the robot
chassis.arcade(lefty, rightx); //this and the code above are used for double stick arcade drive, you should not need to change it.
pros::delay(20); // Sets a delay of 20 milliseconds for the robot; do not change
}