public void exec() {
// pull data from vision
double[] xyScaledDisplacement = robot().GetVisionCore().GetBallDisplacementFromCenter();
// need to invert the X goal, so the robot heads towards the ball (not away)
// for example: if the ball is to the left (negative), the robot needs to move to the left
// (negative)
// if the ball is high (negative, since it is already inverted by visionCore), the robot needs
// to move back (negative)
xyScaledDisplacement[0] = -xyScaledDisplacement[0];
xyScaledDisplacement[1] = -xyScaledDisplacement[1];
// give data to drive
// goal, current, pid
double[] goal = new double[] {0, 0};
double[] power =
followPid.calculate(
goal, xyScaledDisplacement, FlyingBallP.get(), FlyingBallI.get(), FlyingBallD.get());
// use PID? or just raw driving.
// Don't use field-oriented drive here - too many frames of reference would need to change.
// instead, just use robot-relative drive, since the location of the ball is robot-relative.
robot()
.GetDriveCore()
.mecanumDrive_Cartesian(
power[0], power[1], 0, robot().GetSensorCore().GetDefaultOrientation());
// Verbose("Robot followed ball");
}
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());
}
