TachoNavigator turn bug

This is where you talk about the NXJ software itself, installation issues, and programming talk.

Moderators: 99jonathan, roger, imaqine

TachoNavigator turn bug

Postby vladra » Sun Nov 18, 2007 6:03 am

First example:
Code: Select all
TachoNavigator _navigator = ...
_navigator.setPosition(0f, 0f, 0);
_navigator.turn(-30f, 180);

After this there will be the following coordinates:
angle = -180
x = 0
y = 60

It's right.


Second example:
Code: Select all
TachoNavigator _navigator = ...
_navigator.setPosition(0f, 0f, 90);   // or _navigator.rotate(90)
_navigator.turn(-30f, 180);

After this there will be the following coordinates:
angle = -90
x = -60
y = 0

It's wrong! X should be +60
vladra
New User
 
Posts: 24
Joined: Fri Nov 16, 2007 6:18 pm
Location: Russia

Postby vladra » Sun Nov 18, 2007 8:07 am

TachoNavigator.updatePosition, line 349:
Code: Select all
projection = moveAngle + (float)Math.toRadians(_heading); // angle to project displacement onto world coordinates


It's wrong. Should be the following:
Code: Select all
projection = moveAngle - (float)Math.toRadians(_heading); // angle to project displacement onto world coordinates
vladra
New User
 
Posts: 24
Joined: Fri Nov 16, 2007 6:18 pm
Location: Russia

Postby CoBB » Sun Nov 18, 2007 6:04 pm

I worked out the formulae to do tacho navigation from scratch, as I am struggling to build a mapmaking robot (has a colour sensor attached to its nose, pointing to the floor), and it requires as much accuracy as possible. I came up with the following formulae:
Code: Select all
  int dl = <tacho displacement on the left>;
  int dr = <tacho displacement on the right>;

  // Moving along a straight line
  if (dl == dr) {
    cx += wheelCircumference / 360 * dl * cos(dir);
    cy += wheelCircumference / 360 * dl * sin(dir);
    return;
  }

  // Moving along a curve
  double a = wheelCircumference / 360 / wheelDistance * (dl - dr);
  double r;

  if (dl < dr) {
    // Turning leftwards
    if (0 > dr) {
      // Reversing while turning leftwards with the centre outside (to the right)
      r = wheelDistance * (dr / (dr - dl) - 0.5);
    } else {
      // Turning leftwards
      r = wheelDistance * (dl / (dr - dl) + 0.5);
    }
  } else {
    // Turning rightwards
    if (0 > dl) {
      // Reversing while turning rightwards with the centre outside (to the left)
      r = -wheelDistance * (dl / (dl - dr) - 0.5);
    } else {
      // Turning rightwards
      r = -wheelDistance * (dr / (dl - dr) + 0.5);
    }
  }

  cx -= r * (sin(dir + a) - sin(dir));
  cy += r * (cos(dir + a) - cos(dir));
  dir += a;

This is C code running on the PC (the tacho readings are transmitted over bluetooth about 20 times per second), and the persistent variables are cx, cy and dir (angle in radians). As you can see, this can be calculated by evaluating sin and cos exactly once per iteration (if we save sin/cos(dir+a) for the next round), which might be useful when doing all this in the brick.

The question is if there is something I’m overlooking. It seems to work well in general, but I’m not sure if it’s completely correct. After experimenting a bit with the help of a compass it seems that the ‘effective wheel circumference’ varies with the turn radius. :?
User avatar
CoBB
Novice
 
Posts: 73
Joined: Tue Apr 24, 2007 12:24 pm

Postby roger » Thu Nov 22, 2007 5:12 am

Thanks for the bug report and the the suggestion about simpler way to to the calculations. There is a good chance that the next release of NXJ will have a better TachoNavigator.
Roger
roger
Moderator
 
Posts: 350
Joined: Fri Jun 01, 2007 4:31 am
Location: Berkeley, CA

Postby vladra » Sun Nov 25, 2007 4:22 pm

TachoNavigator with my fix is still invalid.

This class is java implementation of CoBB code.

Code: Select all
import lejos.navigation.Navigator;
import lejos.navigation.Pilot;
import lejos.nxt.Motor;

public class AdvancedTachoNavigator implements Navigator
{
    private float _x = 0;
    private float _y = 0;

    private float _a = 0;       // in radians
    private double _sina = 0;   // sin(0) = 0
    private double _cosa = 1;   // cos(0) = 1

    private int _lastLeft = 0;  // last processed wheel rotation angle
    private int _lastRight = 0;

    protected Pilot _pilot;

    public AdvancedTachoNavigator(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor, boolean reverse)
    {
        _pilot = new Pilot(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
    }

    public AdvancedTachoNavigator(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor)
    {
        _pilot = new Pilot(wheelDiameter, trackWidth, leftMotor, rightMotor);
    }

    public AdvancedTachoNavigator(Pilot pilot)
    {
        _pilot = pilot;
    }

    public AdvancedTachoNavigator(float wheelDiameter, float driveLength)
    {
        this(wheelDiameter, driveLength, Motor.A, Motor.C);
    }

    public Pilot getPilot()
    {
        return _pilot;
    }

    public float getX()
    {
        return _x;
    }

    public float getY()
    {
        return _y;
    }

    public float getAngle()
    {
        return (float) Math.toDegrees(_a);
    }

    public float getAngleInRadians()
    {
        return _a;
    }

    public void setPosition(float x, float y, float directionAngle)
    {
        setPositionInRadians(x, y, (float) Math.toRadians(directionAngle));
    }

    public synchronized void setPositionInRadians(float x, float y, float directionAngleInRadians)
    {
        _x = x;
        _y = y;
        _a = directionAngleInRadians;
        _sina = Math.sin(directionAngleInRadians);
        _cosa = Math.cos(directionAngleInRadians);
    }

    public void setSpeed(int speed)
    {
        _pilot.setSpeed(speed);
    }

    public void forward()
    {
        resetTachoCount();
        _pilot.forward();
    }

    public void backward()
    {
        resetTachoCount();
        _pilot.backward();
    }

    public void stop()
    {
        _pilot.stop();
        updatePosition();
    }

    public boolean isMoving()
    {
        return _pilot.isMoving();
    }

    public void travel(float distance)
    {
        travel(distance,false);
    }

    public void travel(float distance,boolean immediateReturn)
    {
        resetTachoCount();
        _pilot.travel(distance,immediateReturn);
        if(!immediateReturn) updatePosition();
    }

    public void rotateLeft()
    {
        resetTachoCount();
        _pilot.steer(200);
    }

    public void rotateRight()
    {
        resetTachoCount();
        _pilot.steer(-200);
    }

    public void rotate(float angle)
    {
        rotate(angle,false);
    }

    public void rotate(float angle,boolean immediateReturn)
    {
        int turnAngle = Math.round(normalize(angle));
        resetTachoCount();
        _pilot.rotate(turnAngle,immediateReturn);
        if(!immediateReturn) updatePosition();
    }

    public void rotateTo(float angle)
    {
        rotateTo(angle,false);
    }

    public void rotateTo(float angle,boolean immediateReturn)
    {
        float turnAngle = normalize( angle - getAngle());
        rotate(turnAngle,immediateReturn);
    }

    public void goTo(float x, float y)
    {
        rotateTo(angleTo(x,y));
        travel(distanceTo(x,y));
    }

    public void goTo(float x, float y, boolean immediateReturn)
    {
        rotateTo(angleTo(x,y));
        travel(distanceTo(x,y), immediateReturn);
    }

    public float distanceTo( float x, float y)
    {
        float dx = x - _x;
        float dy = y - _y;
        //use hypotenuse formula
        return (float)Math.sqrt(dx*dx+dy*dy);
    }

    public float angleTo(float x, float y)
    {
        float dx = x - _x;
        float dy = y - _y;
        return(float)Math.toDegrees(Math.atan2(dy,dx));
    }
   
    public void turn(float radius)
    {
        resetTachoCount();
        _pilot.steer(turnRate(radius));
    }

    public void turn(float radius, int angle)
    {
        turn(radius,angle,false);
    }

    public void turn(float radius, int angle, boolean immediateReturn)
    {
        resetTachoCount();
        _pilot.steer(turnRate(radius),angle,immediateReturn);
        if(!immediateReturn) updatePosition();
    }

    public void updatePosition()
    {
        if( _pilot.getLeftCount()==_lastLeft && _pilot.getRightCount()==_lastRight )
            return;     // this position already has been processed

        try{Thread.sleep(70);}
        catch(InterruptedException e){}

        updatePositionImmediately();
    }

    public synchronized void updatePositionImmediately()
    {
        int left = _pilot.getLeftCount();
        int right = _pilot.getRightCount();

        int dl = left - _lastLeft;
        int dr = right - _lastRight;

        if( dl == 0 && dr == 0 )
            return; // no movement

        _lastLeft = left;
        _lastRight = right;

        // Moving along a straight line
        if (dl == dr)
        {
            _x += dl * _cosa / _pilot._degPerDistance;
            _y += dl * _sina / _pilot._degPerDistance;
            return;
        }

        // Moving along a curve
        float da = (dr-dl) / (_pilot._trackWidth * _pilot._degPerDistance);
        float radius;
        if( dl<dr )
        {   // Turning leftwards
            if( dr<0 )
            {   // Reversing while turning leftwards with the centre outside (to the right)
                radius = (float) (_pilot._trackWidth * (dr / (dr - dl) - 0.5));
            }
            else
            {   // Turning leftwards
                radius = (float) (_pilot._trackWidth * (dl / (dr - dl) + 0.5));
            }
        }
        else
        {   // Turning rightwards
            if( dl<0 )
            {   // Reversing while turning rightwards with the centre outside (to the left)
                radius = (float) (-_pilot._trackWidth * (dl / (dl - dr) - 0.5));
            } else
            {   // Turning rightwards
                radius = (float) (-_pilot._trackWidth * (dr / (dl - dr) + 0.5));
            }
        }

        float newa = normalizeInRadians(_a + da);
        double newsina = Math.sin(newa);
        double newcosa = Math.cos(newa);
        _x += radius * (newsina - _sina);
        _y -= radius * (newcosa - _cosa);
        _a = newa;
        _sina = newsina;
        _cosa = newcosa;
    }

    private float normalize(float angleInDegrees)
    {
        float a = angleInDegrees;
        while(a > 180) a -= 360;
        while(a < -180) a += 360;
        return a;
    }

    private float normalizeInRadians(float angleInRadians)
    {
        float a = angleInRadians;
        while(a > Math.PI) a -= 6.283185307179586;      // PI*2 = 6.283185307179586
        while(a < -Math.PI) a += 6.283185307179586;
        return a;
    }

    private int turnRate(float radius)
    {
        int direction = 1;
        if(radius<0)
        {
            direction = -1;
            radius = -radius;
        }
        float ratio = (2*radius - _pilot._trackWidth)/(2*radius+_pilot._trackWidth);
        return Math.round(direction * 100*(1 - ratio));
    }

    private void resetTachoCount()
    {
        _pilot.resetTachoCount();
        _lastLeft = 0;
        _lastRight = 0;
    }
}


I have made one change. My vehicle has ultrasonic sensor. It is necessary to detect a few objects around. For that ultrasonic sensor detect distance to objects when vehicle is turned.
You can see new updatePositionImmediately method. It allows me to calculate brick position when it is still run.

What do you think? Is it decision fine?
vladra
New User
 
Posts: 24
Joined: Fri Nov 16, 2007 6:18 pm
Location: Russia

Postby roger » Mon Nov 26, 2007 12:24 am

Hi Vladra,
I like the idea of updatePositionImmediately() . Your code looks very efficient, for example calculating sine & cos only once, define _a in radians , etc.

The delay in updatePosition()
was to be sure that the Pilot has stopped before the update. Probably better would be while(pilot.isMoving());

I have not tested the code, but is there a confusion between degrees and radians?
float da = (dr-dl) / (_pilot._trackWidth * _pilot._degPerDistance);
would appear to calculate da in degrees, but
float newa = normalizeInRadians(_a + da);
assumes that da is in radians.

Roger
roger
Moderator
 
Posts: 350
Joined: Fri Jun 01, 2007 4:31 am
Location: Berkeley, CA

Postby vladra » Mon Nov 26, 2007 5:09 am

da is in radians. First time it looks strange for me, but I verify it analytical and in practice. This class work properly. Thanks to CoBB.
vladra
New User
 
Posts: 24
Joined: Fri Nov 16, 2007 6:18 pm
Location: Russia

Postby CoBB » Mon Nov 26, 2007 8:16 am

roger wrote:I have not tested the code, but is there a confusion between degrees and radians?
float da = (dr-dl) / (_pilot._trackWidth * _pilot._degPerDistance);
would appear to calculate da in degrees, but
float newa = normalizeInRadians(_a + da);
assumes that da is in radians.

That’s because dr and dl (the change of tacho readings since the last time) are in degrees, and this formula calculates the turn angle in radians directly from them (note that degrees cancel out in the fraction as we multiply the motor rotation with the 360th of the circumference).
User avatar
CoBB
Novice
 
Posts: 73
Joined: Tue Apr 24, 2007 12:24 pm

Postby vladra » Tue Dec 04, 2007 5:10 pm

There are bug in updatePositionImmediately. Formulas for radius calculation aren't correct.
dr (or dl) has to be casted to double before division.
One of them for example:
Code: Select all
radius = _pilot._trackWidth * ((double) dr / (dr - dl) - 0.5);
vladra
New User
 
Posts: 24
Joined: Fri Nov 16, 2007 6:18 pm
Location: Russia


Return to NXJ Software

Who is online

Users browsing this forum: No registered users and 2 guests

cron
more stuff