I am trying to use the drive to point command in okapi, but it keeps driving to different points then what I stated, weirdly however when I use turn to point command it works fine here is my odometry code
void odometry(){
while(true){
//gets the values from the gps if data is good
if(gps.get_error() <= .0254){
x_position = gps.get_status().x * 39.3701;
y_position = gps.get_status().y * 39.3701;
}
//otherwise uses odometry
else{
x_position = chassis->getState().x.convert(inch);
y_position = chassis->getState().y.convert(inch);
}
//sets heading equal to inertial (just easier)
global_theta = gps.get_heading();
//sets state for use with chassis functions
chassis->setState({x_position * inch, y_position * inch, global_theta * degree});
//debuging tool
if(Controller1.get_digital(pros::E_CONTROLLER_DIGITAL_A)){
std::cout << x_position << "\n";
std::cout << y_position << "\n";
std::cout << global_theta << "\n";
}
pros::delay(20);
}
}
void initialize() {
pros::lcd::initialize();
pros::lcd::set_text(1, “Hello PROS User!”);
chassis = ChassisControllerBuilder()
.withMotors(8,15,16,2)
.withGains(
{0.0005, 0, 0.00005}, // distance controller gains
{0.006, 0, 0.0001}, // turn controller gains
{0.001, 0, 0.0001} // angle controller gains (helps drive straight)
)
.withSensors(
ADIEncoder{'A', 'B'}, // left encoder in ADI ports A & B
ADIEncoder{'G', 'H',true}, // right encoder in ADI ports C & D (reversed)
ADIEncoder{'C', 'D', true} // middle encoder in ADI ports E & F
)
.withLogger(
std::make_shared<Logger>(
TimeUtilFactory::createDefault().getTimer(), // It needs a Timer
"/ser/sout", // Output to the PROS terminal
Logger::LogLevel::debug // Most verbose log level
)
)
// green gearset, tracking wheel diameter (2.75 in), track (7 in), and TPR (360)
// 1 inch middle encoder distance, and 2.75 inch middle wheel diameter
.withOdometryTimeUtilFactory(ConfigurableTimeUtilFactory(5,10,50_ms))
.withDimensions(AbstractMotor::gearset::blue, {{2.75_in, 11_in, 5.9_in, 2.75_in}, quadEncoderTPR})
.withOdometry() // use the same scales as the chassis (above)
.buildOdometry(); // build an odometry chassis;
pros::Task odom(odometry);
}
void test1(){
chassis->driveToPoint({0_in,22_in});
pros::delay(200);
chassis->driveToPoint({22_in,22_in});
pros::delay(200);
chassis->driveToPoint({22_in, 0_in});
pros::delay(200);
chassis->driveToPoint({0_in,0_in});
pros::delay(200);
}
I tried doing turn to point and then drive distance instead, but that didn’t work either. The odometry is also returning the right position so I don’t know what is creating the deviance,
