Okapi drive to point not going to right point

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,

Does it reliably drive to the same location when started from the same position?

If so, it is possible that the coordinate and heading system reported by the GPS sensor is different than the coordinate and heading conventions used by OkapiLib. The GPS sensor natively returns position based on:

Where up is 0-degrees, and clockwise increases heading. I am not sure what convention OkapiLib uses.

2 Likes

No I’ll start it from the same place and it goes to different points