void SideConfidenceProvider::update(SideConfidence& sideConfidence) { // Debug stuff DECLARE_PLOT("module:SideConfidenceProvider:originalWeighting"); DECLARE_PLOT("module:SideConfidenceProvider:mirroredWeighting"); // Setting the time when the robot has a good stand if(theFallDownState.state != theFallDownState.upright && theFallDownState.state != theFallDownState.undefined && theFallDownState.state != theFallDownState.staggering && theFrameInfo.getTimeSince(timeOfLastFall) > 8000) timeOfLastFall = theFrameInfo.time; // Setting the time when the robot not has arm contact if(!theArmContactModel.contactLeft && !theArmContactModel.contactRight) lastTimeWithoutArmContact = theFrameInfo.time; // Remove old entries from confidence buffer while(!confidenceBuffer.empty()) { if(theFrameInfo.getTimeSince(confidenceBuffer.back().timeStamp) > maxBufferAge) confidenceBuffer.pop_back(); else break; } updateBallConfidences(sideConfidence); updateSideConfidenceFromOthers(sideConfidence); updateSideConfidenceFromOwn(sideConfidence); updateConfidenceState(sideConfidence); // MOOOOH! if(sideConfidence.mirror) SystemCall::playSound("theMirrorCow.wav"); }
void SideConfidenceProvider::update(SideConfidence& sideConfidence) { MODIFY("parameters:SideConfidenceProvider", parameters); updateBallConfidences(); updateSideConfidenceFromOthers(sideConfidence); updateSideConfidenceFromOwn(sideConfidence); handleAloneAndConfused(sideConfidence); updateConfidenceState(sideConfidence); // Setting some timings lastFrameTime = theFrameInfo.time; // Setting the time when not in own penalty area if(!(theRobotPose.translation.x < theFieldDimensions.xPosOwnPenaltyArea && abs(theRobotPose.translation.y) < theFieldDimensions.yPosLeftPenaltyArea)) timeWhenEnteredOwnPenaltyArea = theFrameInfo.time; // Setting the distance walked when not having a bad validity if(theRobotPose.validity > parameters.halfPercentage) lastDistanceWalkedAtHighValidity = theOdometer.distanceWalked; // Setting the time when the robot has a good stand if((theFallDownState.state != theFallDownState.upright && theFallDownState.state != theFallDownState.undefined && theFallDownState.state != theFallDownState.staggering) && (theFrameInfo.getTimeSince(timeOfLastFall) > 8000) && !robotHasFallen) { timeOfLastFall = theFrameInfo.time; robotHasFallen = true; } // Setting the time when the robot not has arm contact if(!theArmContactModel.contactLeft && !theArmContactModel.contactRight) lastTimeWithoutArmContact = theFrameInfo.time; }