public void drive(double x, double y, double rot) {
l_angle = Math.toDegrees(MathUtils.atan2(-x, -y));
l_magnitude = Math.sqrt((x * x) + (y * y));
forward = y;
right = -x;
clockwise = rot;
// make sure angles are in the expected range
l_angle %= 360;
r_angle %= 360;
if (l_angle < 0) {
l_angle = 360 + l_angle;
}
if (r_angle < 0) {
r_angle = 360 + r_angle;
}
// make sure magnitudes are in range
l_magnitude = Math.max(-1, l_magnitude);
l_magnitude = Math.min(1, l_magnitude);
r_magnitude = Math.max(-1, r_magnitude);
r_magnitude = Math.min(1, r_magnitude);
driveCartesianTest();
}
private void setBrakeMode() {
double sign, angle = MathUtils.atan2(nw, nl);
for (int i = 0; i < 4; i++) {
if (i == FL || i == BR) sign = 1;
else sign = -1;
modules[i].set(angle * sign, 0);
}
}
private void set(double x, double y, double r) {
double m = Math.max(1, Math.max(Math.abs(x), Math.abs(y)));
x /= m;
y /= m;
r = Math.max(-1, Math.min(1, r));
double[] angle = new double[4], power = new double[4];
double cx, cy, maxPower = 1;
for (int i = 0; i < 4; i++) {
if (i == FL || i == FR) cx = x + r * nl;
else cx = x - r * nl;
if (i == FL || i == BL) cy = y + r * nw;
else cy = y - r * nw;
// power[i] = Math.hypot(cx, cy);
power[i] = Math.sqrt(cx * cx + cy * cy);
angle[i] = MathUtils.atan2(cy, cx) - Math.PI / 2;
maxPower = Math.max(power[i], maxPower);
}
for (int i = 0; i < 4; i++)
if (Math.abs(power[i]) < .01) modules[i].stopDrive();
else modules[i].set(angle[i], power[i] / maxPower);
}
protected void calcXY(double x, double y) {
double curX, curY, deltaX, deltaY, calcAngle, calcDistance, robotAngle;
curX = Robot.chassis.getX();
curY = Robot.chassis.getY();
deltaX = x - curX;
deltaY = y - curY;
calcDistance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
calcAngle = Math.toDegrees(MathUtils.atan2(deltaX, deltaY));
if (Double.isNaN(calcAngle) || Double.isInfinite(calcAngle)) {
System.err.println("Infinite calcAngle in DriveWaypoint");
calcAngle = 0;
}
robotAngle = Robot.chassis.getAngle();
if (Double.isNaN(robotAngle) || Double.isInfinite(robotAngle)) {
System.err.println("Infinite robotAngle in DriveWaypoint");
robotAngle = 0;
}
if (Math.abs(robotAngle - calcAngle) < 180) {
// do nothing
} else if (robotAngle > calcAngle) {
while (robotAngle > calcAngle) calcAngle += 360;
} else {
while (robotAngle < calcAngle) calcAngle -= 360;
}
// System.out.println("distance: " + calcDistance);
// System.out.println("angle: " + calcAngle);
super.setParam1(calcDistance);
super.setParam3(calcAngle);
}
public void drive() {
/*
* Matt, I found some incredibly simple code to drive a mecanum drive
* system with two joysticks (one for x, y cartesian coordinates and
* one for rotation here: http://www.chiefdelphi.com/forums/showthread.php?t=89205&highlight=mecanum+normalize
* I have a suspicion that your code implements this in a roundabout
* way by calculating angles, etc. I am not sure if this works with
* a tank drive control scheme, but I can imagine that it can be
* adopted if it works out.
*
*
// 3-axis joystick interface to a mecanum or omni drive
// first define your driver interface,
// in this case a 3-axis joystick:
forward = -Y; // push joystick forward to go forward
right = X; // push joystick to the right to strafe right
clockwise = Z; // twist joystick clockwise turn clockwise
// here is where you would put any special shaping of the joystick
// response curve, such as deadband or gain adjustment
// now apply the inverse kinematic tranformation
// to convert your vehicle motion command
// to 4 wheel speed commands:
// NOTE: you can introduce some tuning parameters for forward,
// rotate and strafing to dial in the correct feel. See example
// below.
front_left = forward + clockwise + right;
front_right = forward - clockwise - right;
rear_left = forward + clockwise - right;
rear_right = forward - clockwise + right;
// finally, normalize the wheel speed commands
// so that no wheel speed command exceeds magnitude of 1:
max = abs(front_left);
if (abs(front_right)>max) max = abs(front_right);
if (abs(rear_left)>max) max=abs(rear_left);
if (abs(rear_right)>max) max=abs(rear_right);
if (max>1)
{front_left/=max; front_right/=max; rear_left/=max; rear_right/=max;}
// you're done. send these four wheel commands to their respective wheels
*
*
*
*
* Found some tank drive psuedo code which is similar:
*
Here's a way to program TANK DRIVE on a mecanum bot so that you
can tune the joystick sensitivity to all three motions (fwd/rev,
turn, stafe) independently:
Let Kf, Kt, and Ks be the tuning parameters (0 to +1) for the
forward/reverse, turn, and strafe motions, respectively.
Let X1 and Y1 represent the joystick outputs for the driver's
left-hand joystick (-1 to +1);
Let Y2 represent the joystick outputs for the driver's
right-hand joystick (-1 to +1).
When each joystick is pushed forward, its Y output should be
positive. When the joystick is pushed to the right, its X output
should be positive. If not, add code to invert the sign if
necessary.
Let W1, W2, W3, and W4 be the front left, front right, rear
left, and rear right wheels, respectively. ("left" and "right"
in this context means "port" and "starboard", respectively)
Calculate the following:
Yf = (Y1 + Y2)/2
Yt = (Y1 - Y2)/2
Now calculate the four wheel speed commands:
W1 = Kf*Yf + Kt*Yt + Ks*X1
W2 = Kf*Yf - Kt*Yt - Ks*X1
W3 = Kf*Yf + Kt*Yt - Ks*X1
W4 = Kf*Yf - Kt*Yt + Ks*X1
Now normalize the wheel speed commands:
Let Wmax be the maximum absolute value of the four wheel speed
commands. If Wmax is greater than 1, then divide each of the four
wheel speed commands by Wmax.
Finally, send each of the four normalized wheel speed commands
to the respective wheels (-1 means 100% reverse, +1 means 100% forward).
The Y1 and Y2 axes act like tank drive. The X1 axis commands
strafe left and right. The X2 axis is not used.
Tune Kf, Kt, and Ks (from 0 to +1) to get the desired joystick
sensitivity to each of the three motions.
*
* Could you try this out in your simulation? Thanks.
*
* - Mr. Ward
*/
/*
* NOTE: To those of you who don't understand what is happening here
* allow me to explain:
*
* We are computing the angle and maginitude of each joystick position
* by utilizing trigonometric properties, specifically this one:
*
* /|
* / |
*hyp / |
* / | opposite, y or axis 1
* / |
* / i |
* /______|
* adjacent, x or axis 2
*
* SOA CAH TOA
*
* sin(i) = opposite / hypotenous
* cos(i) = adjacent / hypotenous
* tan(i) = opposite / adjacent
*
* i = arctan( opposite / adacent )
*
* With the magnitude we are simply calculating the hypotenous via the
* formula:
*
* hyp^2 = x^2 + y^2
* hyp = sqrt( x^2 + y^2 )
*
*/
RRLogger.logDebug(this.getClass(), "drive()", "");
double l_xVal = m_xboxStick.getRawAxis(1);
double l_yVal = m_xboxStick.getRawAxis(2);
double r_xVal = m_xboxStick.getRawAxis(4);
double r_yVal = m_xboxStick.getRawAxis(5);
if (Math.abs(l_xVal) < .13) {
l_xVal = 0;
}
if (Math.abs(l_yVal) < .13) {
l_yVal = 0;
}
if (Math.abs(r_xVal) < .13) {
r_xVal = 0;
}
if (Math.abs(r_yVal) < .13) {
r_yVal = 0;
}
// Change the range of the joystick values to account for the dead zone
if (l_xVal > 0) {
l_xVal = (l_xVal - .13) / (1 - .13);
} else if (l_xVal < 0) {
l_xVal = (l_xVal + .13) / (1 - .13);
}
if (l_yVal > 0) {
l_yVal = (l_yVal - .13) / (1 - .13);
} else if (l_yVal < 0) {
l_yVal = (l_yVal + .13) / (1 - .13);
}
if (r_xVal > 0) {
r_xVal = (r_xVal - .13) / (1 - .13);
} else if (r_xVal < 0) {
r_xVal = (r_xVal + .13) / (1 - .13);
}
if (r_yVal > 0) {
r_yVal = (r_yVal - .13) / (1 - .13);
} else if (r_yVal < 0) {
r_yVal = (r_yVal + .13) / (1 - .13);
}
l_angle =
Math.toDegrees(MathUtils.atan2(-m_xboxStick.getRawAxis(1), -m_xboxStick.getRawAxis(2)));
l_magnitude =
Math.sqrt(
(m_xboxStick.getRawAxis(1) * m_xboxStick.getRawAxis(1))
+ (m_xboxStick.getRawAxis(2) * m_xboxStick.getRawAxis(2)));
r_angle =
Math.toDegrees(MathUtils.atan2(-m_xboxStick.getRawAxis(4), -m_xboxStick.getRawAxis(5)));
r_magnitude =
Math.sqrt(
(m_xboxStick.getRawAxis(4) * m_xboxStick.getRawAxis(4))
+ (m_xboxStick.getRawAxis(5) * m_xboxStick.getRawAxis(5)));
if (l_magnitude < .28) {
l_magnitude = 0;
}
if (r_magnitude < .28) {
r_magnitude = 0;
}
rotation = -m_xboxStick.getRawAxis(3);
forward = l_yVal;
right = -l_xVal;
clockwise = m_xboxStick.getRawAxis(3);
right_forward = r_yVal;
right_right = -r_xVal;
// make sure angles are in the expected range
l_angle %= 360;
r_angle %= 360;
if (l_angle < 0) {
l_angle = 360 + l_angle;
}
if (r_angle < 0) {
r_angle = 360 + r_angle;
}
// make sure magnitudes are in range
l_magnitude = Math.max(-1, l_magnitude);
l_magnitude = Math.min(1, l_magnitude);
r_magnitude = Math.max(-1, r_magnitude);
r_magnitude = Math.min(1, r_magnitude);
// decrease magnitude for precise mode
if (m_xboxStick.getRawButton(5)) {
if (m_xboxStick.getRawButton(6)) {
l_magnitude *= .4;
r_magnitude *= .4;
rotation *= .4;
forward *= .4;
right *= .4;
clockwise *= .4;
right_forward *= .4;
right_right *= .4;
sensitivity = .4;
} else {
l_magnitude *= .7;
r_magnitude *= .7;
rotation *= .7;
forward *= .7;
right *= .7;
clockwise *= .7;
right_forward *= .7;
right_right *= .7;
sensitivity = .7;
}
}
// Toggle Through Drive Modes with Start Button
if (m_xboxStick.getRawButton(8) && !controlModeSwitched) {
switch (controlMode) {
case DRIVE_MECANUM:
controlMode = DRIVE_TANK;
break;
case DRIVE_TANK:
controlMode = DRIVE_CARTESIAN_TEST;
break;
case DRIVE_CARTESIAN_TEST:
controlMode = DRIVE_CARTESIAN_TANK;
break;
case DRIVE_CARTESIAN_TANK:
controlMode = DRIVE_MECANUM;
break;
}
controlModeSwitched = true;
} else if (!m_xboxStick.getRawButton(8)) {
controlModeSwitched = false;
}
// drive in correct mode
switch (controlMode) {
case DRIVE_MECANUM:
driveMecanum();
break;
case DRIVE_TANK:
driveTank();
break;
case DRIVE_CARTESIAN_TEST:
driveCartesianTest();
break;
case DRIVE_CARTESIAN_TANK:
driveCartesianTank();
break;
}
/*
RRLogger.logDebug(this.getClass(),"drive()","FLE Distance: "+ fl_Encoder.get());
RRLogger.logDebug(this.getClass(),"drive()","FRE Distance: "+ fr_Encoder.get());
RRLogger.logDebug(this.getClass(),"drive()","BLE Distance: "+ bl_Encoder.get());
RRLogger.logDebug(this.getClass(),"drive()","BRE Distance: "+ br_Encoder.get());
*/
}
/**
* This method returns the angle (in degrees) of the vector pointing from Origin to Measured
* projected to the YZ plane. If the mirrored parameter is true the vector is flipped about the
* Y-axis. Mirroring is used to avoid the region where the atan2 function is discontinuous
*
* @param origin The Skeleton Joint to use as the origin point
* @param measured The Skeleton Joint to use as the endpoint of the vector
* @param mirrored Whether to mirror the Z coordinate of the joint about the Y-axis
* @return The angle in degrees
*/
public double AngleYZ(Skeleton.Joint origin, Skeleton.Joint measured, boolean mirrored) {
return Math.toDegrees(
MathUtils.atan2(
measured.getY() - origin.getY(),
(mirrored) ? (origin.getZ() - measured.getZ()) : (measured.getZ() - origin.getZ())));
}
public static double round(double num, int decimalPlaces) {
double places = MathUtils.pow(10, decimalPlaces);
double newDouble = num * places;
int newInt = (int) newDouble;
return newInt / places;
}