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 }