public void exec() {
if (robot().GetDriveCore().tankFieldOriented.get()) {
// If the rotation joystick is being actuated, use that. Otherwise use the heading maintainer.
XYPair rightJoy =
new XYPair(robot().GetOICore().getRightJoyX(), robot().GetOICore().getRightJoyY());
double rotation = 0;
if (rightJoy.internalAbsoluteSum() > tankDriveDeadbandRightJoystick.get()) {
rotation = DriveCore.remapMinusDeadband(rightJoy.X, tankDriveDeadbandRightJoystick.get());
// Set the desired heading to the current heading so we don't thrash once the joystick is
// released.
robot().GetDriveCore().SetDesiredHeading(robot().GetSensorCore().getCurrentHeading());
} else {
// We'll first get the joystick vector, and set a desired
// heading for the robot which maximizes speed towards that vector.
this.orientForSpeed.exec();
// Get the heading we should drive the arcade at from the PID.
// This value is the rotation speed that we pass to the arcade drive.
rotation = this.headingMaintainer.MaintainHeading();
}
// Our translation speed should be the magnitude of the joystick vector,
// projected onto the robot heading.
// So when the robot is at 90 degrees to the joystick heading,
// it rotates rather than translating.
// Conversely, when the robot is exactly the same as the joystick
// heading, the rotation speed should be 0 and the translation speed should be 1.
// Magnitude will be capped at 1.
double currentRobotHeading = robot().GetSensorCore().getCurrentHeading();
XYPair robotOrientation = XYPair.fromPolarCoordinates(1, currentRobotHeading);
double movement = robotOrientation.project(robot().GetOICore().getLeftJoyVector());
if (movement > 1.0) {
movement = 1.0;
} else if (movement < -1.0) {
movement = -1.0;
}
robot().GetDriveCore().arcadeDrive(movement, rotation);
} else {
robot()
.GetDriveCore()
.tankDrive(robot().GetOICore().getLeftJoyY(), robot().GetOICore().getRightJoyY());
}
}
public void exec() {
XYPair curPos = robot().GetSensorCore().odometry.GetFieldOrientedPosition();
double[] output =
pidArray.calculate(
target.toArray(),
curPos.toArray(),
translationPositionP.get(),
translationPositionI.get(),
translationPositionD.get());
double rotationalForce = headingMaintainer.MaintainHeading();
robot()
.GetDriveCore()
.mecanumDrive_Cartesian(
output[0], output[1], rotationalForce, robot().GetSensorCore().getCurrentHeading());
}