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");
}
/** @return */
public ExecState isFinished() {
boolean currentOnTarget = pidArray.isOnTarget(0.5);
if (!previousOnTarget && currentOnTarget) {
enteredOnTargetMarker = CommonTools.Get().time.GetMarker();
} else if (previousOnTarget
&& currentOnTarget
&& CommonTools.Get().time.GetElapsedMSecFromMarker(enteredOnTargetMarker)
> onTargetWaitTimeMS) {
return ExecState.SUCCESS;
}
previousOnTarget = currentOnTarget;
return ExecState.NOT_DONE;
}
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());
}
public void init() {
headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
headingMaintainer.init();
pidArray.reset();
}
/** @param target */
public void setSetpoint(XYPair target) {
pidArray.reset();
this.target = target;
}