Odometry not turning the correct way

lerker100 · March 18, 2020, 2:38pm

Chassis declaration

Motor frontLeftMotor(9, false, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor backLeftMotor(10, false, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor frontRightMotor(2, true, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor backRightMotor(3, true, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
std::shared_ptr<OdomChassisController> chassis = ChassisControllerBuilder()
	.withMotors(frontLeftMotor, frontRightMotor, backRightMotor, backLeftMotor)
  .withMaxVelocity(150)
  .withSensors(leftEncoder, rightEncoder)
    .withGains(
        {0.0035, 0, 0}, // Distance controller gains
        {0.003, 0, 0}, // Turn controller gains
        {0.002, 0, 0.00006}  // Angle controller gains (helps drive straight)
    )
    .withDimensions(AbstractMotor::gearset::green, {{3.5_in, 23.5_in}, imev5BlueTPR})
    .withOdometry() // use the same scales as the chassis (above)
    .buildOdometry(); // build an odometry chassis

Auton code

  chassis->setState({0_in, 0_in, 0_deg});
  chassis->setMaxVelocity(100);
  Roller.moveVelocity(200);
  chassis->driveToPoint({4.5_ft, 0_ft});
  chassis->driveToPoint({1_ft, 0_ft});
  Roller.moveVelocity(0);
  chassis->driveToPoint({0.7_ft, -0.4_ft});

When I run this code the robot follows the lines on the photo. When it gets to were the yellow and green lines touch instead of making a small turn it turns all the way around to face the goal zone. It also does this when I set the backwards flag to true.

rpm · March 18, 2020, 8:47pm

It looks like you telling the controller there are optical shaft encoders but then you don’t tell odometry what those dimensions are. So if you are using encoders attached to tracking wheels then follow this example :

ChassisControllerBuilder()
    .withMotors(1, -2) // left motor is 1, right motor is 2 (reversed)
    // green gearset, 4 inch wheel diameter, 11.5 inch wheelbase
    .withDimensions(AbstractMotor::gearset::green, {{4_in, 11.5_in}, imev5GreenTPR})
    .withSensors(
        ADIEncoder{'A', 'B'}, // left encoder in ADI ports A & B
        ADIEncoder{'C', 'D', true}  // right encoder in ADI ports C & D (reversed)
    )
    // specify the tracking wheels diameter (2.75 in), wheel track (7 in), and TPR (360)
    .withOdometry({{2.75_in, 7_in}, quadEncoderTPR})
    .buildOdometry();

If you don’t have separate encoder and tracking wheels follow this example,

ChassisControllerBuilder()
    .withMotors(1, -2) // left motor is 1, right motor is 2 (reversed)
    // green gearset, 4 inch wheel diameter, 11.5 inch wheelbase
    .withDimensions(AbstractMotor::gearset::green, {{4_in, 11.5_in}, imev5GreenTPR})
    .withOdometry() // use the same scales as the chassis (above)
    .buildOdometry(); // build an odometry chassis

in either case fix teh green gearset with the blue IME.

btw, i’d get it working with the internal encoders and PID first, then if that is not good enough use the tracking wheels and chassisControllerPID.

lerker100 · March 18, 2020, 8:50pm

I changed the size and cartridges in the code because I’m using mechanum drive and if I set the actual values it didn’t drive the correct distances. Is there a better way to adjust that?

rpm · March 18, 2020, 11:05pm

My mecanum wheels (as is most of my kit) are on loan so I cannot test if they are treated differently than omni’s or traction. However, I too have problems with distance being exact, but the distance is consistent each run.

I haven’t been using odometry but I’ll try to find some time tonight to add it in and see if it impacts turning. Though turning is an other thing that is not even close to the value selected but it is consistent.

rpm · March 19, 2020, 5:27am

TL:DR the turn is as expected.

Ok, so I ran your drive steps and log shows it doing what you noted. You can try using turnAngle to see if you can get to turn in the positive direction with a smaller angle.

What it is doing is calculating atan of (about) -.4/-.3 which is -127 deg and then subtracting the current heading of 177 (ideally it would have been 180) from it (-127 -177) or about -304. Of course odom is using the end position from the encoders so the 0-.4 and .7-1 are the ideal number which is why the log shows -307. I guess they could optimize it by looking to see if if adding 360 or subtracting 360 would result in an smaller magnitude (or any other method). note i’m using 4" wheels and a track of 10.5".

8854 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Computed length of 1.371600 meters and angle of 0.000000 degrees
8854 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Driving 1.371600 meters
8854 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: moving 1.371600 meters
…
10794 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: Done waiting to settle
10794 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Computed length of 1.064269 meters and angle of 177.878093 degrees
10794 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Turning 177.878093 degrees
10794 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: turning 177.878093 degrees
…
11824 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: Done waiting to settle
11824 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Driving 1.064269 meters
11824 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: moving 1.064269 meters
…
13484 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: Done waiting to settle
13484 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Computed length of 0.142188 meters and angle of -307.331413 degrees
13484 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Turning -307.331413 degrees
13484 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: turning -307.331413 degrees
…
14824 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: Done waiting to settle
14824 (User Operator Control (PROS)) INFO: DefaultOdomChassisController: Driving 0.142188 meters
14824 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: moving 0.142188 meters
…
15594 (User Operator Control (PROS)) INFO: ChassisControllerIntegrated: Done waiting to settle

ed: grammar

lerker100 · March 19, 2020, 4:41pm

I had tried turnAngle witch did turn correctly but when I told it to drive to the last point it would than turn 360 than drive to the goal. I might go look at the code for this and see if I can get it to not do that. What debugging were you using to get that output?

rpm · March 19, 2020, 5:54pm

with either the brain or the controller connected to usb. (sometimes i need to unplug usb from pc and replug into another usb slot to get this working).
cmd prompt> prosv5 terminal
or from the IDE (look for icon with hover text of “terminal”. but don’t like this option since the terminal sometimes closes before I can review it.

and add the logger to your chassis as shown at the bottom of the tutorial.
https://okapilib.github.io/OkapiLib/md_docs_tutorials_walkthrough_chassisControllerBuilder.html

I used “info” but “debug” shows more.

Mentor_355U · March 19, 2020, 6:58pm

This seems like a desirable optimization; what would be involved in executing it?

rbenasutti · March 19, 2020, 8:35pm

This is the algorithm:

github.com

Team-Optimistic/Team_Optimistic/blob/d6b11f7d5a9e58c72e2c5dd9d944369602bc20a7/turningFunctions.c#L19-L23


      
          	//Fix angle
          	while(angle>180)
                angle-=360;
            while(angle<=-180)
                angle+=360;

I am not sure where I want it to be implemented. There are two levels: the OdomChassisController implementation (which computes the angle to turn) and the ChassisController implementation (which actually does the turn). I can imagine arguments for both, though it might make more sense to implement this at the ChassisController level, in which case you will need to modify these two files:

github.com

OkapiLib/OkapiLib/blob/master/src/api/chassis/controller/chassisControllerPid.cpp#L157


      
            moveDistanceAsync(itarget);
            waitUntilSettled();
          }
          
          void ChassisControllerPID::moveRaw(const double itarget) {
            // Divide by straightScale so the final result turns back into motor ticks
            moveDistance((itarget / scales.straight) * meter);
          }
          
          void ChassisControllerPID::turnAngleAsync(const QAngle idegTarget) {
            LOG_INFO("ChassisControllerPID: turning " + std::to_string(idegTarget.convert(degree)) +
                     " degrees");
            LOG_DEBUG("ChassisControllerPID: scales.turn " + std::to_string(scales.turn) + " ratio " +
                      std::to_string(gearsetRatioPair.ratio));
          
            turnPid->reset();
            turnPid->flipDisable(false);
            distancePid->flipDisable(true);
            anglePid->flipDisable(true);
            mode = angle;

github.com

OkapiLib/OkapiLib/blob/master/src/api/chassis/controller/chassisControllerIntegrated.cpp#L83


      
            waitUntilSettled();
          }
          
          void ChassisControllerIntegrated::turnRaw(const double idegTarget) {
            // Divide by turnScale so the final result turns back into motor ticks
            turnAngle((idegTarget / scales.turn) * degree);
          }
          
          void ChassisControllerIntegrated::turnAngleAsync(const QAngle idegTarget) {
            LOG_INFO("ChassisControllerIntegrated: turning " + std::to_string(idegTarget.convert(degree)) +
                     " degrees");
          
            leftController->reset();
            rightController->reset();
            leftController->flipDisable(false);
            rightController->flipDisable(false);
          
            const double newTarget =
              idegTarget.convert(degree) * scales.turn * gearsetRatioPair.ratio * boolToSign(normalTurns);
          
            LOG_INFO("ChassisControllerIntegrated: turning " + std::to_string(newTarget) + " motor ticks");

I will accept a PR for this change. You will, of course, also need to write some tests to reproduce the behavior, and then fix the code so that the tests pass

rpm · March 20, 2020, 2:27am

@rbenasutti I can look at it, but I’m really not a software engineer just someone who knows how to read an API. Maybe I can talk @Barin into it. He’s supper sharp.

Sylvie · March 20, 2020, 4:19am

You sure 'bou dat?

Barin · March 20, 2020, 4:36am

Well, I have literally never programmed anything for V5, in PROS or otherwise. Nor do I have any hardware for testing or the time to work on this…

So I’m going to pass this on to @sazrocks

By the way, this describes me too

lerker100 · March 20, 2020, 1:45pm

I have a 4 motor mechanum drive robot with two external encoders on the back wheels.

lerker100 · March 20, 2020, 2:04pm

@rbenasutti The first link that you gave is to a different repo. How does that code tie into the okapilib code? And I don’t know how to apply any changes that I make to my project to test on hardware.

rbenasutti · March 20, 2020, 3:00pm

The first link is the algorithm that should be implemented in OkapiLib.

Mentor_355U · March 20, 2020, 6:21pm

Non-looping method to constrain an angle:

double normalizedAngle = angle - (ceil((angle + M_PI)/(2*M_PI))-1)*2*M_PI;  // (-Pi;Pi]:
double normalizedAngle = angle - (ceil((angle + 180)/360)-1)*360;           // (-180;180]:

tabor473 · March 20, 2020, 6:43pm

Its just one of those times where you choose between concise and verbose in software. Concise often can be opaque and verbose might be needlessly long.

rbenasutti · March 20, 2020, 6:44pm

If you want to use that method instead, I am fine with it.

rpm · March 20, 2020, 7:08pm

Am I missing something, it looks like the user input theta (angle from above code) is the only point where it can be greater than 360? If the code is using atan2 - theta then the loop will only every run once unless the user provides a starting theta greater than +/-360?

rbenasutti · March 20, 2020, 7:14pm

You aren’t missing anything.