I am using pros and lemlib to code autonomous. After a move to point function followed by a turn to heading function, my robot refuses to move backwards. Can some please me? Thanks`#include “main.h”
#include “lemlib/api.hpp” // IWYU pragma: keep
using namespace pros;
using namespace lemlib;
pros::MotorGroup right_mg({14,11,-17},pros::MotorGearset::blue);
pros::MotorGroup left_mg({4,-5,-10},pros::MotorGearset::blue);
pros::adi::Pneumatics mogoalclamp(‘a’,false);
pros::adi::Pneumatics doinker(‘b’,false);
pros::MotorGroup elevator_mg({-3,12});
pros::MotorGroup wallstakes_mg({-19});
pros::Rotation ws_rotation({8});
pros::Rotation vertical_encoder(2);
pros::Rotation horizontal_encoder(6);
lemlib::Drivetrain drivetrain(&left_mg,&right_mg,12.9,lemlib::Omniwheel::NEW_325,360,2);
pros::Imu imu(1);
lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_encoder, lemlib::Omniwheel::NEW_2, 1);
lemlib::TrackingWheel vertical_tracking_wheel(&vertical_encoder, lemlib::Omniwheel::NEW_2,2);
lemlib::OdomSensors sensors(&vertical_tracking_wheel,nullptr,&horizontal_tracking_wheel, nullptr,&imu);
// lateral PID controller
lemlib::ControllerSettings lateral_controller(21, // proportional gain (kP)
0, // integral gain (kI)
4, // derivative gain (kD)
0, // anti windup
0, // small error range, in inches
0, // small error range timeout, in milliseconds
0, // large error range, in inches
0, // large error range timeout, in milliseconds
20 // maximum acceleration (slew)
);
lemlib::ControllerSettings angular_controller(6, // proportional gain (kP)
0, // integral gain (kI)
30, // derivative gain (kD)
0, // anti windup
0, // small error range, in inches
0, // small error range timeout, in milliseconds
0, // large error range, in inches
0, // large error range timeout, in milliseconds
20 // maximum acceleration (slew)
);
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);
}
}
/**
- 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::register_btn1_cb(on_center_button);
}
/**
- 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() {
/**
-
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.
*/
/*pros::MotorGroup right_mg({-14,-11,17},pros::MotorGearset::blue);
pros::MotorGroup left_mg({4,-5,-10},pros::MotorGearset::blue);
pros::adi::Pneumatics mogoalclamp(‘a’,false);
pros::adi::Pneumatics doinker(‘b’,false);
pros::MotorGroup elevator_mg({-3,12});
pros::MotorGroup wallstakes_mg({-19});
pros::Rotation ws_rotation({8});
wallstakes_mg.set_brake_mode_all(pros::E_MOTOR_BRAKE_HOLD);
pros::Rotation vertical_encoder(2);
pros::Rotation horizontal_encoder(6);
lemlib::Drivetrain drivetrain(&left_mg,&right_mg,12.9,lemlib::Omniwheel::NEW_325,360,2);
pros::Imu imu(1);
lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_encoder, lemlib::Omniwheel::NEW_2, 3,2);
lemlib::TrackingWheel vertical_tracking_wheel(&vertical_encoder, lemlib::Omniwheel::NEW_2,2,3);
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)
);*/
}
void opcontrol() {
pros::delay(1000);
wallstakes_mg.set_brake_mode_all(pros::E_MOTOR_BRAKE_HOLD);
chassis.calibrate();
chassis.setPose(0, 0, 0);
chassis.moveToPoint(0, -0.9, 1000, {.forwards = true, .maxSpeed = 50}, true);
pros::delay(1000);
chassis.turnToHeading(90, 1000, {.maxSpeed = 50}, true);
pros::delay(1000);
chassis.setPose(0, 0, 0);
chassis.moveToPoint(0, 2, 1000, {.forwards = false, .maxSpeed = 50}, true);
pros::delay(1000);
chassis.moveToPoint(-2, 0.9, 1000, {.forwards = false, .maxSpeed = 50}, true);
/*pros::Controller master(pros::E_CONTROLLER_MASTER);
pros::MotorGroup right_mg({-14,-11,17}); // 14,11,17 // Creates a motor group with forwards ports 1 & 3 and reversed port 2
pros::MotorGroup left_mg({4,-5,-10});//4,5,10
pros::adi::Pneumatics mogoalclamp('a',false);
pros::adi::Pneumatics doinker('b',false);
pros::MotorGroup elevator_mg({-3,12});
pros::MotorGroup wallstakes_mg({-19});
wallstakes_mg.set_brake_mode_all(pros::E_MOTOR_BRAKE_HOLD);
pros::Rotation ws_rotation({8});
ws_rotation.set_reversed(true);
ws_rotation.reset();*/
//while (true) {
//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 turn = master.get_analog(ANALOG_LEFT_Y); // Gets amount forward/backward from left joystick
int dir = master.get_analog(ANALOG_RIGHT_X); // Gets the turn left/right from right joystick
left_mg.move(dir - turn); // Sets left motor voltage
right_mg.move(dir + turn); // Sets right motor voltage
if (master.get_digital(pros::E_CONTROLLER_DIGITAL_L1)){
mogoalclamp.set_value(true);
}else if(master.get_digital(pros::E_CONTROLLER_DIGITAL_L2)){
mogoalclamp.set_value(false);}
if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R1)){
elevator_mg.move(500);
}else if(master.get_digital(pros::E_CONTROLLER_DIGITAL_R2)){
elevator_mg.move(-500);}
else{
elevator_mg.brake();
}
if (master.get_digital(pros::E_CONTROLLER_DIGITAL_UP)){
doinker.set_value(true);
}else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_DOWN)){
doinker.set_value(false);}
else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_X)){
wallstakes_mg.move(80);
}else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_B)){
wallstakes_mg.move(-80);
}else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_Y)){
pros::lcd::print(0, "%d %d %d",ws_rotation.get_angle());
while (ws_rotation.get_angle()<35076){
wallstakes_mg.move(80);}
wallstakes_mg.brake();
}else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_A)){
ws_rotation.reset();}
else{
wallstakes_mg.brake();
}
if (master.get_digital(pros::E_CONTROLLER_DIGITAL_RIGHT)){
elevator_mg.move_absolute(-30,-20);
}*/
//pros::delay(20);
//}
}`