bool Collide::circleRect(const Position& circleCenter,
Position::value_type circleRadius,
const Shape& rect) {
// TODO: actually algorithm can ran on any convex polygon, just need to generalize it
REQUIRE(TAG, rect.geometry().points() == 4, "Rect must have four points");
Position a = rect.transformation().position() + rect.geometry().pointAt(0);
Position b = rect.transformation().position() + rect.geometry().pointAt(1);
Position c = rect.transformation().position() + rect.geometry().pointAt(2);
Position d = rect.transformation().position() + rect.geometry().pointAt(3);
return pointRect (circleCenter, a, b, c, d)
|| lineCircle(circleCenter, circleRadius, a, b)
|| lineCircle(circleCenter, circleRadius, b, c)
|| lineCircle(circleCenter, circleRadius, c, d)
|| lineCircle(circleCenter, circleRadius, d, a);
}
void AvcNav::steer () {
pickWaypoint();
if (numWaypointsSet > 0) {
int h = getHeadingToWaypoint();
if (h >= 0 && goodHdop) {
bestKnownHeading = h;
steerHeading = bestKnownHeading;
#if USE_LINE_INTERSECT
// if (!reorienting) {
steerHeading = lineCircle(latitude, longitude, sLat, sLon, dLat, dLon);
// }
#endif
} else {
steerHeading = bestKnownHeading;
}
} else {
bestKnownHeading = 0;
}
// float cte = crossTrackError();
// there might be a problem with fixtime and previous fix time if this code runs a long time. or maybe even a short time.
// if (gpsUpdated) {
// pidOffset = pid.compute(cte, (fixTime - previousFixTime)/1000.0, odometerSpeed);
// previousPidOffset = pidOffset;
// }
int steering = SERVO_CENTER;
int adj = 0;
if (!goodHdop || reorienting || COMPASS_ONLY || odometerSpeed < 1 || DISTANCE_FROM_LINE_CORRECTION) {
pidOffset = 0;
int headingOffset = (heading - steerHeading + 360) % 360;
if (abs(heading - bestKnownHeading) < 30) {
reorienting = false;
}
#if AGRESSIVE_STEERING
if(headingOffset >= 180) {
headingOffset = constrain(360 - (360 - headingOffset) * 2, 180, 360);
steering = map(headingOffset, 180, 360, MAX_SERVO_75_PERCENT, SERVO_CENTER);
} else {
headingOffset = constrain(headingOffset * 2, 0, 180);
steering = map(headingOffset, 180, 0, MIN_SERVO_75_PERCENT, SERVO_CENTER);
}
#else
if(headingOffset >= 180) {
steering = map(headingOffset, 180, 360, MAX_SERVO, SERVO_CENTER);
} else {
steering = map(headingOffset, 180, 0, MIN_SERVO, SERVO_CENTER);
}
#endif
byte maxSteeringDiff = percentOfServo(.75);//(.05);
//#if DISTANCE_FROM_LINE_CORRECTION
//// float cte = crossTrackError();
// // if approaching the line then allow for faster steering
//// if (abs(cte) < abs(previousCte)) {
// maxSteeringDiff = percentOfServo(.3);
//// }
// if (cte > 0) {
// adj = int(min(percentOfServo(.2), int(cte * percentOfServo(.05))));
// } else {
// adj = int(max(-percentOfServo(.2), int(cte * percentOfServo(.05))));
// }
// previousCte = cte;
//// Serial << bestKnownHeading << "\t";
//// Serial << heading << "\t";
//// Serial << cte << "\t";
//// Serial << adj << "\t";
//// Serial << steering - SERVO_CENTER << "\t";
// steering = constrain(steering + adj, MIN_SERVO, MAX_SERVO);
//// Serial << steering - SERVO_CENTER << endl;
//#endif
// if (steering - previousSteering > maxSteeringDiff && steering > SERVO_CENTER) {
// steering = previousSteering + maxSteeringDiff;
// } else if (steering - previousSteering < -maxSteeringDiff && steering < SERVO_CENTER) {
// steering = previousSteering - maxSteeringDiff;
// }
previousSteering = steering;
//#if !COMPASS_ONLY
// } else { // use pid
// if (!gpsUpdated) {
// pidOffset = previousPidOffset;
// }
// if(pidOffset <= 0) {
// steering = map(pidOffset, -100, 0, MAX_SERVO, SERVO_CENTER);
// } else {
// steering = map(pidOffset, 100, 0, MIN_SERVO, SERVO_CENTER);
// }
// const byte maxSteeringDiff = 2;
// if (steering - previousSteering > maxSteeringDiff) {
// steering = previousSteering + maxSteeringDiff;
// } else if (steering - previousSteering < -maxSteeringDiff) {
// steering = previousSteering - maxSteeringDiff;
// }
// previousSteering = steering;
// goodHdop = checkHdop();
//#endif
}
goodHdop = checkHdop();
#if LOG_HEADING
logHeadingData(bestKnownHeading, steering - SERVO_CENTER, /*cte*/0.0, adj);
#endif
#if LOG_EKF
logEkfData();
#endif
#if USE_SERVO_LIBRARY
#if GO_STRAIGHT
steeringServo.writeMicroseconds(SERVO_CENTER);
#else
steeringServo.writeMicroseconds(steering);
#endif
#else
analogWrite(SERVO_PIN, steering);
#endif
}
