/**
* 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 returns the number of motors in the drive train.
*
* @return number of motors.
*/
public int getNumMotors() {
final String funcName = "getNumMotors";
if (debugEnabled) {
dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API);
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API, "=%d", numMotors);
}
return numMotors;
} // getNumMotors
/**
* This method sets the maximum output value of the motor.
*
* @param maxOutput specifies the maximum output value.
*/
public void setMaxOutput(double maxOutput) {
final String funcName = "setMaxOutput";
if (debugEnabled) {
dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API, "maxOutput=%f", maxOutput);
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
this.maxOutput = maxOutput;
} // setMaxOutput
/**
* This method sets the sensitivity for the drive() method.
*
* @param sensitivity specifies the sensitivity value.
*/
public void setSensitivity(double sensitivity) {
final String funcName = "setSensitivity";
if (debugEnabled) {
dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API, "sensitivity=%f", sensitivity);
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
this.sensitivity = sensitivity;
} // setSensitivity
/**
* 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 stops all the motors. */
public void stopMotor() {
final String funcName = "stopMotor";
if (debugEnabled) {
dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API);
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
if (frontLeftMotor != null) frontLeftMotor.setPower(0.0);
if (frontRightMotor != null) frontRightMotor.setPower(0.0);
if (rearLeftMotor != null) rearLeftMotor.setPower(0.0);
if (rearRightMotor != null) rearRightMotor.setPower(0.0);
} // stopMotor
/**
* 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
/**
* This method inverts direction of a given motor in the drive train.
*
* @param motorType specifies the motor in the drive train.
* @param isInverted specifies true if inverting motor direction.
*/
public void setInvertedMotor(MotorType motorType, boolean isInverted) {
final String funcName = "setInvertedMotor";
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"type=%s,inverted=%s",
motorType.toString(),
Boolean.toString(isInverted));
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
switch (motorType.value) {
case MotorType.kFrontLeft_val:
if (frontLeftMotor != null) {
frontLeftMotor.setInverted(isInverted);
}
break;
case MotorType.kFrontRight_val:
if (frontRightMotor != null) {
frontRightMotor.setInverted(isInverted);
}
break;
case MotorType.kRearLeft_val:
if (rearLeftMotor != null) {
rearLeftMotor.setInverted(isInverted);
}
break;
case MotorType.kRearRight_val:
if (rearRightMotor != null) {
rearRightMotor.setInverted(isInverted);
}
break;
}
} // setInvertedMotor
/**
* This method drives the motors with the given magnitude and curve values.
*
* @param magnitude specifies the magnitude value.
* @param curve specifies the curve value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void drive(double magnitude, double curve, boolean inverted) {
final String funcName = "drive";
double leftOutput;
double rightOutput;
if (debugEnabled) {
dbgTrace.traceEnter(
funcName,
TrcDbgTrace.TraceLevel.API,
"mag=%f,curve=%f,inverted=%s",
magnitude,
curve,
Boolean.toString(inverted));
dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
}
if (curve < 0.0) {
double value = Math.log(-curve);
double ratio = (value - sensitivity) / (value + sensitivity);
if (ratio == 0.0) {
ratio = 0.0000000001;
}
leftOutput = magnitude / ratio;
rightOutput = magnitude;
} else if (curve > 0.0) {
double value = Math.log(curve);
double ratio = (value - sensitivity) / (value + sensitivity);
if (ratio == 0.0) {
ratio = 0.0000000001;
}
leftOutput = magnitude;
rightOutput = magnitude / ratio;
} else {
leftOutput = magnitude;
rightOutput = magnitude;
}
tankDrive(leftOutput, rightOutput, inverted);
} // drive
