void Dribbler::step(float dt) {
Wheel::step(dt);
double delayStopPeriod = 0.1;
if (stopRequestedTime != -1.0 && Util::duration(stopRequestedTime) >= delayStopPeriod) {
setTargetOmega(0);
//setTargetOmega(-Config::robotDribblerSpeed / 5);
stopRequestedTime = -1.0;
}
if (ballDetected) {
ballInDribblerTime += dt;
if (ballInDribblerTime >= Config::ballInDribblerThreshold) {
ballLostTime = -1.0f;
}
} else {
if (everDetectedBall) {
if (ballLostTime == -1.0f) {
ballLostTime = dt;
} else {
ballLostTime += dt;
}
}
if (ballLostTime >= Config::dribblerBallLostThreshold) {
ballInDribblerTime = 0.0f;
}
}
if (isRaiseRequested) {
timeSinceRaised += dt;
timeSinceLowered = 0.0f;
} else {
timeSinceLowered += dt;
timeSinceRaised = 0.0f;
}
// apply limits just in case periodically
if (timeSinceLimitsApplied > 1.0f) {
applyLimits();
}
timeSinceLimitsApplied += dt;
useChipKickLimitsMissedFrames++;
/*if (useChipKickLimitsMissedFrames >= 2) {
useNormalLimits();
}*/
//std::cout << "ballInDribblerTime: " << ballInDribblerTime << ", ballLostTime: " << ballLostTime << ", got ball: " << (gotBall() ? "yes" : "no") << std::endl;
}
void Wheel::setTargetSpeed(int speed) {
setTargetOmega(speedToOmega((float)speed));
}
void Robot::lookAt(const Math::Angle &angle) {
setTargetOmega(Math::limit(angle.rad() * Config::lookAtP, Math::degToRad(Config::lookAtMaxSpeedAngle) * Config::lookAtP));
}
