/** This function is called in every execution cycle of the simulation*/
void update()
{
detectBall();
// Oh behave:
if(vehicleState == SEARCH_FOR_BALL)
{
frontLeftWheel->setValue(M_PI/1.5);
frontRightWheel->setValue(-M_PI/1.5);
backLeftWheel->setValue(M_PI/1.5);
backRightWheel->setValue(-M_PI/1.5);
if(ballFound)
{
simRobot.setStatusMessage("Ball detected. Driving to ball.");
vehicleState = GO_TO_BALL;
}
}
else if(vehicleState == GO_TO_BALL)
{
driveToBall();
if(ballFound == false)
{
simRobot.setStatusMessage("Lost ball. Searching ball again.");
vehicleState = SEARCH_FOR_BALL;
}
}
}
void ImageProcessor::processFrame(){
if(vblocks_.robot_state->WO_SELF == WO_TEAM_COACH && camera_ == Camera::BOTTOM) return;
visionLog((30, "Process Frame camera %i", camera_));
updateTransform();
// Horizon calculation
visionLog((30, "Calculating horizon line"));
HorizonLine horizon = HorizonLine::generate(iparams_, cmatrix_, 30000);
vblocks_.robot_vision->horizon = horizon;
visionLog((30, "Classifying Image", camera_));
if(!classifier_->classifyImage(color_table_)) return;
detectBall();
}
///btActionInterface interface
virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime){
detectBall(collisionWorld);
dribblerStep();
kickerStep();
}
