}
public void steer(final float turnRate, final float angle,
final boolean immediateReturn) {
// TODO: make this work with wheels of different size
TachoMotor inside;
TachoMotor outside;
float rate = turnRate;
if (rate < -200) {
rate = -200;
}
if (rate > 200) {
rate = 200;
}
if (rate == 0) {
if (angle < 0) {
backward();
} else {
forward();
}
return;
}
if (turnRate < 0) {
inside = _right;
outside = _left;
rate = -rate;
} else {
inside = _left;
outside = _right;
}
outside.setSpeed(_motorSpeed);
float steerRatio = 1 - rate / 100.0f;
inside.setSpeed((int) (_motorSpeed * steerRatio));
if (angle == Integer.MAX_VALUE) // no limit angle for turn
{
if (_parity == 1) {
outside.forward();
} else {
outside.backward();
}
if (_parity * steerRatio > 0) {
inside.forward();
} else {
inside.backward();
}
return;
}
float rotAngle = angle * _trackWidth * 2
/ (_leftWheelDiameter * (1 - steerRatio));
inside.rotate(_parity * (int) (rotAngle * steerRatio), true);
outside.rotate(_parity * (int) rotAngle, immediateReturn);
if (immediateReturn) {
return;
}
while (inside.isMoving() || outside.isMoving())
// changed isRotating() to isMoving() as this covers what we need
// and alows us to keep the interface small
Thread.yield();
inside.setSpeed(outside.getSpeed());
}