Odometry not turning the correct way

#1

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.
IMG_20200317_130028_1

2 Likes
#2

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.

2 Likes
#3

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?

#4

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.

3 Likes
#5

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

2 Likes
#6

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?

#7

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.

2 Likes
#8

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

2 Likes
#9

This is the algorithm:

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:


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 :slight_smile:

4 Likes
#10

@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.

#11

You sure 'bou dat?

1 Like
#12

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 :slight_smile:

By the way, this describes me too :upside_down_face:

5 Likes
#13

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

#14

@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.

#15

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

2 Likes
#16

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]:
1 Like
#17

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.

1 Like
#18

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

1 Like
#19

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?

#20

You aren’t missing anything.