/** * Controller::reset() * * These values should only be reset once. They set the robot position as (0,0) with a heading of 0 radians. */ void Controller::reset() { _fHeadingRef = 0.0; _fHeadingError = 0.0; _fHeadingErrorPrev = 0.0; _fHeadingErrorIntegral = 0.0; _fPosXRef = 0.0; _fPosYRef = 0.0; _currentPose.x = 0.0; _currentPose.y = 0.0; _currentPose.heading = 0.0; _currentPose.timestamp = 0; resetDistance(); }
task main() { writeValueToLEDs(0); autonomousOptimized = true; initSwerve(); initLift(); lineFollowerInit(); writeValueToLEDs(IDLE); waitForStart(); initGyro(); setModulePositions(1440); resetDistance(); while ( !driveToCenter(false) ) { updateGyro(); updateSwerve(); updateLift(); } }
/** * Controller::goToPosition * * @param double x desired waypoint x co-ord * @param double y desired waypoing y co-ord * @param double fPosXStated believed current x co-ord * @param double fPosYStated believed current y co-ord * * @return void */ void Controller::goToPosition(double x, double y, double fPosXStated, double fPosYStated) { double fTargetVectorMagnitude; // current distance between where we think we are and the waypoint double fTargetVectorMagnitudeLast = 0; // last distance between where we think we were and the waypoint double fTargetVectorMagnitudeInitial = 0; // initial distance between where we think we are and the waypoint double fTargetVectorMagnitudeAtStartOfDrift; // if we begin to get further from the target (rather than closer, perhaps due to a turning circle) what was our distance to the target when this started? unsigned int nConsecutiveIncreasingDistance = 0; // number of times the distance has increased rather than decrease (used to stop out of controlness) bool bApproachingTarget = false; // once we start approaching the target don't forget it double fVelocityLeft = 0, fVelocityRight = 0; // current velocity of left and right wheels Led ledHealth(1), ledProximity(3); ledHealth.off(); ledProximity.off(); // Reset the distance counters that are used in the PID loop, they are relative to our last (this) waypoint resetDistance(); _logger->notice("goToPosition: --- START ---\nGoing to position (%.2f, %.2f) from current position (%.2f, %.2f) and heading [%.2f] ...", x, y, _currentPose.x, _currentPose.y, _currentPose.heading); _fPosXRef = x; _fPosYRef = y; // We need to keep our heading as close to this reference heading as possible to reach the waypoint _fHeadingRef = getHeading(_fPosXRef, _fPosYRef, _currentPose.x, _currentPose.y, _currentPose.heading); // _fHeadingRef = getHeading(_fPosXRef, _fPosYRef, fPosXStated, fPosYStated, _fHeadingCurrent); _logger->notice("goToPosition: new heading required is [%.2f]", _fHeadingRef); // Start (and reset) the odometry thread _odo->reset(); int iteration = 0; bool bFirstIteration = true; struct timeval tvNow, tvLast; int nsec; unsigned long delayms, totaldelayms = 0; // time since last iteration and total time in this waypoint gettimeofday(&tvLast, NULL); while (true) { /** * Recalculate our reference heading every now and again as our current position changes. */ if (iteration % 10 == 0) { _logger->notice("goToPosition: recalculating heading based on current position..."); _fHeadingRef = getHeading(_fPosXRef, _fPosYRef, _currentPose.x, _currentPose.y, _currentPose.heading); _logger->notice("new heading is %.2f", _fHeadingRef); } gettimeofday(&tvNow, NULL); if (tvNow.tv_usec < tvLast.tv_usec) { nsec = (tvLast.tv_usec - tvNow.tv_usec) / 1000000 + 1; tvLast.tv_usec -= 1000000 * nsec; tvLast.tv_sec += nsec; } if (tvNow.tv_usec - tvLast.tv_usec > 1000000) { nsec = (tvLast.tv_usec - tvNow.tv_usec) / 1000000; tvLast.tv_usec += 1000000 * nsec; tvLast.tv_sec -= nsec; } // How long did we sleep since the last iteration? Required for derivatives. delayms = ((tvNow.tv_sec - tvLast.tv_sec) * 1000) + ((tvNow.tv_usec - tvLast.tv_usec) / 1000); totaldelayms += delayms; // total time in this waypoint segment _totalTime += delayms; // total time transiting altogether tvLast = tvNow; _currentPose.timestamp = _totalTime; // How long have we slept? This is required for our integral and derivative PID values. double dt = delayms / 1000.0; ledHealth.strobe(); _logger->notice("goToPosition: %lu --- ITERATION %d --- \nreference: (%.2f, %.2f, distance: %.2f) at heading %.2f (total runtime: %lu) (dt: %.2f)", delayms, iteration, _fPosXRef, _fPosYRef, fTargetVectorMagnitudeInitial, _fHeadingRef, totaldelayms, dt); // How far has each wheel travelled? This is total distance since odo reset (start of waypoint). if (_bSimulation) { // Don't use the odometer, see what our model predicts. This can be used to determine how accurate the odometer and drive train is. _fDistLeft += dt * fVelocityLeft; _fDistRight += dt * fVelocityRight; } else { _odo->getDistance(&_fDistLeft, &_fDistRight); } _fDistTotal = (_fDistLeft + _fDistRight) / 2.0; _logger->notice("goToPosition: distL[%.2f] distR[%.2f] dist[%.2f] distPrev[%.2f]", _fDistLeft, _fDistRight, _fDistTotal, _fDistTotalPrev); // How far have we travelled in this last iteration? double fDistDelta = _fDistTotal - _fDistTotalPrev; double fDistLeftDelta = _fDistLeft - _fDistLeftPrev; double fDistRightDelta = _fDistRight - _fDistRightPrev; // Position has changed based on the distance travelled at the previous heading _currentPose.x += fDistDelta * cos(_currentPose.heading); _currentPose.y += fDistDelta * sin(_currentPose.heading); // Update the heading as it has changed based on the distance travelled too _currentPose.heading += ((fDistRightDelta - fDistLeftDelta) / CONTROLLER_WHEELBASE); // Ensure our heading remains sane _currentPose.heading = atan2(sin(_currentPose.heading), cos(_currentPose.heading)); _fDistTotalPrev = _fDistTotal; _fDistLeftPrev = _fDistLeft; _fDistRightPrev = _fDistRight; // What's the error between our required heading and our heading? double fHeadingErrorRaw = _fHeadingRef - _currentPose.heading; _fHeadingError = atan2(sin(fHeadingErrorRaw), cos(fHeadingErrorRaw)); // How fast should we proceed forward? (Anything below 5 results in non-movement due to inertia) double fForwardVelocity = 9.0; // 5.0 works as well but undershoots // What is the magnitude of the vector between us and our target? fTargetVectorMagnitude = sqrt(((_fPosXRef - _currentPose.x)*(_fPosXRef - _currentPose.x)) + ((_fPosYRef - _currentPose.y)*(_fPosYRef - _currentPose.y))); if (bFirstIteration) { fTargetVectorMagnitudeInitial = fTargetVectorMagnitude; } _logger->notice("goToPosition: heading[%.4f, e: %.6f] posX[%.2f] posY[%.2f] distToTarget[%.2f] distToTargetLast[%.2f]", _currentPose.heading, _fHeadingError, _currentPose.x, _currentPose.y, fTargetVectorMagnitude, fTargetVectorMagnitudeLast); /** * @todo: Come up with a better way of determining whether we should consider ourself "close enough" to the waypoint. */ if ( ! bFirstIteration) { if (fTargetVectorMagnitude >= fTargetVectorMagnitudeLast) { if (nConsecutiveIncreasingDistance == 0) { fTargetVectorMagnitudeAtStartOfDrift = fTargetVectorMagnitude; } nConsecutiveIncreasingDistance++; double fDistanceCoveredSinceStartOfDrift = fTargetVectorMagnitude - fTargetVectorMagnitudeAtStartOfDrift; if (fDistanceCoveredSinceStartOfDrift >= (0.25 * fTargetVectorMagnitudeInitial)) // if (nConsecutiveIncreasingDistance > MAX_ITERATIONS_OF_INCREASING_TARGET_VECTOR_BEFORE_TERMINATION) { _logger->notice("goToPosition: Distance to target has increased!\ngoToPosition: --- END ABNORMAL ---\n"); break; } } else { nConsecutiveIncreasingDistance = 0; } } if (fTargetVectorMagnitude < 10 || bApproachingTarget) { bApproachingTarget = true; ledProximity.on(); _logger->notice("goToPosition: Approaching target, slowing down."); fForwardVelocity = 3.0; // 3.0 works as well, safer } // _logger->notice("goToPosition: x(%.2f)\ty(%.2f)\ttheta(%.2f)", _fPosXCurrent, _fPosYCurrent, _fHeadingCurrent); if (fTargetVectorMagnitude <= 5) { _logger->notice("goToPosition: You have arrived at your destination!\ngoToPosition: --- END ---\n"); _dotLogPosition->log((_totalTime / 1000.0), _currentPose.x, _currentPose.y, DotLog::DotLogPositionColour::RED, true); break; } else { _dotLogPosition->log((_totalTime / 1000.0), _currentPose.x, _currentPose.y, bApproachingTarget ? DotLog::DotLogPositionColour::RED : DotLog::DotLogPositionColour::BLACK); } fTargetVectorMagnitudeLast = fTargetVectorMagnitude; // if (fabs(fPosXCurrent - fPosXRef) < (CONTROLLER_WHEELBASE/2.0) && fabs(fPosYCurrent - fPosYRef) < (CONTROLLER_WHEELBASE/2.0)) // { // printf("You have arrived at your destination (non-vector)!\n"); // break; // } // Maintain the PID variables double fHeadingErrorDerivative = (_fHeadingError - _fHeadingErrorPrev) / dt; _fHeadingErrorIntegral += (_fHeadingError * dt); _fHeadingErrorPrev = _fHeadingError; double pidP = CONTROLLER_PID_PROPORTIONAL * _fHeadingError; double pidI = CONTROLLER_PID_INTEGRAL * _fHeadingErrorIntegral; double pidD = CONTROLLER_PID_DERIVATIVE * fHeadingErrorDerivative; // PID, this gives us the control signal, u, this is required angular velocity double u = pidP + pidI + pidD; // Angular velocity gives us new wheel velocities. We assume a constant forward velocity for simplicity. fVelocityRight = ((2.0*fForwardVelocity) + (u*CONTROLLER_WHEELBASE)) / (2.0*CONTROLLER_WHEELRADIUS); // cm/s fVelocityLeft = ((2.0*fForwardVelocity) - (u*CONTROLLER_WHEELBASE)) / (2.0*CONTROLLER_WHEELRADIUS); // cm/s _logger->notice("goToPosition: P[%.6f] I[%.6f] D[%.6f] u[%.6f] -> required velocities (left,right) are (%.2f,%.2f)", pidP, pidI, pidD, u, fVelocityLeft, fVelocityRight); // These velocities are relative, work out the required velocity per wheel. // float vLeft = (fVelocityLeft / (fabs(fVelocityLeft) + fabs(fVelocityRight))) * 100.0; // float vRight = (fVelocityRight / (fabs(fVelocityLeft) + fabs(fVelocityRight))) * 100.0; // // int dLeft = static_cast<int>(fabs(vLeft)); // int dRight = static_cast<int>(fabs(vRight)); // // printf("Required relative velocities (left,right) are (%.2f,%.2f) -> (%d,%d)\n", vLeft, vRight, dLeft, dRight); int nLeftPWM = convertVelocityToPWMPercentage(true, fVelocityLeft); int nRightPWM = convertVelocityToPWMPercentage(false, fVelocityRight); // Apply the new velocities to the motors if ( ! _bSimulation) { if (fVelocityLeft < 0.0) { motor_reverse(MOTOR_LEFT, pwm_speed(nLeftPWM)); } else { motor_forward(MOTOR_LEFT, pwm_speed(nLeftPWM)); } if (fVelocityRight < 0.0) { motor_reverse(MOTOR_RIGHT, pwm_speed(nRightPWM)); } else { motor_forward(MOTOR_RIGHT, pwm_speed(nRightPWM)); } } usleep(50000); // 50ms // usleep(300000); // 300 ms // usleep(1000000); // 1s /** * time passes ... wheels respond to new control signal and begin moving at new velocities * * These wheel velocities turn the wheels and influence the distance travelled and heading * which is recalculated in the next iteration. */ bFirstIteration = false; iteration++; } bot_stop(); ledHealth.off(); }
Snake::Snake(const Position &position, sf::Color color, bool rainbow) { resetDistance(); addPiece(position, color, rainbow); }