/**
* This method implements mecanum drive where x controls how fast the robot will go in the x
* direction, and y controls how fast the robot will go in the y direction. Rotation controls how
* fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
*
* @param x specifies the x power.
* @param y specifies the y power.
* @param rotation specifies the rotating power.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
* @param gyroAngle specifies the gyro angle to maintain.
*/
public void mecanumDrive_Cartesian(
double x, double y, double rotation, boolean inverted, double gyroAngle) {
final String funcName = "mecanumDrive_Cartesian";
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"x=%f,y=%f,rot=%f,inverted=%s,angle=%f",
x,
y,
rotation,
Boolean.toString(inverted),
gyroAngle);
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
if (numMotors != MAX_NUM_MOTORS) {
throw new IllegalArgumentException("Mecanum drive requires 4 motors");
}
x = TrcUtil.limit(x);
y = TrcUtil.limit(y);
rotation = TrcUtil.limit(rotation);
if (inverted) {
x = -x;
y = -y;
}
double cosA = Math.cos(Math.toRadians(gyroAngle));
double sinA = Math.sin(Math.toRadians(gyroAngle));
x = x * cosA - y * sinA;
y = x * sinA + y * cosA;
double wheelSpeeds[] = new double[MAX_NUM_MOTORS];
wheelSpeeds[MotorType.kFrontLeft_val] = x + y + rotation;
wheelSpeeds[MotorType.kFrontRight_val] = -x + y - rotation;
wheelSpeeds[MotorType.kRearLeft_val] = -x + y + rotation;
wheelSpeeds[MotorType.kRearRight_val] = x + y - rotation;
normalize(wheelSpeeds);
if (frontLeftMotor != null) {
frontLeftMotor.setPower(wheelSpeeds[MotorType.kFrontLeft_val]);
}
if (frontRightMotor != null) {
frontRightMotor.setPower(wheelSpeeds[MotorType.kFrontRight_val]);
}
if (rearLeftMotor != null) {
rearLeftMotor.setPower(wheelSpeeds[MotorType.kRearLeft_val]);
}
if (rearRightMotor != null) {
rearRightMotor.setPower(wheelSpeeds[MotorType.kRearRight_val]);
}
} // mecanumDrive_Cartesian
/**
* This method implements arcade drive where drivePower controls how fast the robot goes in the
* y-axis and turnPower controls how fast it will turn.
*
* @param drivePower specifies the drive power value.
* @param turnPower specifies the turn power value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void arcadeDrive(double drivePower, double turnPower, boolean inverted) {
final String funcName = "arcadeDrive";
double leftPower;
double rightPower;
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"drivePower=%f,turnPower=%f,inverted=%s",
drivePower,
turnPower,
Boolean.toString(inverted));
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
drivePower = TrcUtil.limit(drivePower);
turnPower = TrcUtil.limit(turnPower);
if (drivePower + turnPower > MOTOR_MAX_VALUE) {
//
// Forward right:
// left = drive + turn - (drive + turn - MOTOR_MAX_VALUE)
// right = drive - turn - (drive + turn - MOTOR_MAX_VALUE)
//
leftPower = MOTOR_MAX_VALUE;
rightPower = -2 * turnPower + MOTOR_MAX_VALUE;
} else if (drivePower - turnPower > MOTOR_MAX_VALUE) {
//
// Forward left:
// left = drive + turn - (drive - turn - MOTOR_MAX_VALUE)
// right = drive - turn - (drive - turn - MOTOR_MAX_VALUE)
//
leftPower = 2 * turnPower + MOTOR_MAX_VALUE;
rightPower = MOTOR_MAX_VALUE;
} else if (drivePower + turnPower < MOTOR_MIN_VALUE) {
//
// Backward left:
// left = drive + turn - (drive + turn - MOTOR_MIN_VALUE)
// right = drive - turn - (drive + turn - MOTOR_MIN_VALUE)
//
leftPower = MOTOR_MIN_VALUE;
rightPower = -2 * turnPower + MOTOR_MIN_VALUE;
} else if (drivePower - turnPower < MOTOR_MIN_VALUE) {
//
// Backward right:
// left = drive + turn - (drive - turn - MOTOR_MIN_VALUE)
// right = drive - turn - (drive - turn - MOTOR_MIN_VALUE)
//
leftPower = 2 * turnPower + MOTOR_MIN_VALUE;
rightPower = MOTOR_MIN_VALUE;
} else {
leftPower = drivePower + turnPower;
rightPower = drivePower - turnPower;
}
tankDrive(leftPower, rightPower, inverted);
} // arcadeDrive
/**
* This method implements mecanum drive where magnitude controls how fast the robot will go in the
* given direction and how fast it will robote.
*
* @param magnitude specifies the magnitude combining x and y axes.
* @param direction specifies the direction in degrees.
* @param rotation specifies the rotation power.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void mecanumDrive_Polar(
double magnitude, double direction, double rotation, boolean inverted) {
final String funcName = "mecanumDrive_Polar";
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"mag=%f,dir=%f,rot=%f,inverted=%s",
magnitude,
direction,
rotation,
Boolean.toString(inverted));
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
if (numMotors != MAX_NUM_MOTORS) {
throw new IllegalArgumentException("Mecanum drive requires 4 motors");
}
magnitude = TrcUtil.limit(magnitude) * Math.sqrt(2.0);
if (inverted) {
direction += 180.0;
direction %= 360.0;
}
double dirInRad = Math.toRadians(direction + 45.0);
double cosD = Math.cos(dirInRad);
double sinD = Math.sin(dirInRad);
double wheelSpeeds[] = new double[MAX_NUM_MOTORS];
wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);
normalize(wheelSpeeds);
if (frontLeftMotor != null) {
frontLeftMotor.setPower(wheelSpeeds[MotorType.kFrontLeft_val]);
}
if (frontRightMotor != null) {
frontRightMotor.setPower(wheelSpeeds[MotorType.kFrontRight_val]);
}
if (rearLeftMotor != null) {
rearLeftMotor.setPower(wheelSpeeds[MotorType.kRearLeft_val]);
}
if (rearRightMotor != null) {
rearRightMotor.setPower(wheelSpeeds[MotorType.kRearRight_val]);
}
} // mecanumDrive_Polar
/**
* This method implements tank drive where leftPower controls the left motors and right power
* controls the right motors.
*
* @param leftPower specifies left power value.
* @param rightPower specifies right power value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void tankDrive(double leftPower, double rightPower, boolean inverted) {
final String funcName = "tankDrive";
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"leftPower=%f,rightPower=%f,inverted=%s",
leftPower,
rightPower,
Boolean.toString(inverted));
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
leftPower = TrcUtil.limit(leftPower);
rightPower = TrcUtil.limit(rightPower);
if (inverted) {
double swap = leftPower;
leftPower = -rightPower;
rightPower = -swap;
}
if (frontLeftMotor != null) {
frontLeftMotor.setPower(leftPower);
}
if (frontRightMotor != null) {
frontRightMotor.setPower(rightPower);
}
if (rearLeftMotor != null) {
rearLeftMotor.setPower(leftPower);
}
if (rearRightMotor != null) {
rearRightMotor.setPower(rightPower);
}
} // tankDrive