void inputManager::update(){
if(targets.size() > 0){
isEmpty = false;
}else{
isEmpty = true;
}
updateTargets();
calcAveragePos();
player.update();
if (player.isFrameNew()){
ofxCv::unwarpPerspective(player, blah, inputQuad);
blah.update();
CVM.update(blah.getPixels());
}
}
void FighterFightTask::update(float deltaSec) {
updateTargets();
updateState();
WorldObject* worldObject = boardComputer()->worldObject();
switch (m_state) {
case State::IDLE:
return;
case State::APPROACH:
if (angleToTarget() > 45.0f) {
boardComputer()->rotateTo(m_primaryTarget->transform().position());
} else {
boardComputer()->rotateTo(m_primaryTarget->transform().position());
boardComputer()->moveTo(m_primaryTarget->transform().position());
}
if (targetDistance() < componentsInfo().maxBulletRange()) {
boardComputer()->shootBullet(m_targets);
}
break;
case State::ENGAGE:
if (angleToTarget() > 45.0f) {
boardComputer()->moveTo(worldObject->transform().position() + glm::vec3(0, 0, -1) * worldObject->transform().orientation());
boardComputer()->rotateTo(m_primaryTarget->transform().position());
} else {
boardComputer()->rotateTo(m_primaryTarget->transform().position());
boardComputer()->moveTo(m_primaryTarget->transform().position());
if (angleToTarget() < 15.0f && targetDistance() < componentsInfo().maxRocketRange()) {
boardComputer()->shootRockets(m_primaryTarget);
}
}
if (targetDistance() < componentsInfo().maxBulletRange()) {
boardComputer()->shootBullet(m_targets);
}
break;
case State::EVADE:
boardComputer()->rotateTo(m_positionBehindTarget);
boardComputer()->moveTo(m_positionBehindTarget);
break;
}
}
bool CProject::update()
{
if (!isValid())
return false;
settingsFileInfo.refresh();
if (lastUpdateTime >= settingsFileInfo.lastModified()) {
return true;
}
updateStr();
updateInclude();
updateLibrary();
updateLdPath();
updateMainSources();
updateMainSourceDepend();
updateTargets();
lastUpdateTime = settingsFileInfo.lastModified();
emit updated(nameStr);
return true;
}
void Robot::planAction(void) {
ownTime_ms_delta_t hurryUp = 1000*60*11; // If it's time to abandon everything else and get current targets to the goal
static ownTime_ms_delta_t explRollStartTime = 0;
static ownTime_ms_delta_t hidasprkl = 0;
if (!manual.enabled) {
if(statistics.taskStartTime == 0)
statistics.taskStartTime = ownTime_get_ms();
SLAM::RobotLocation p = slam.getCurrentMapData().getRobotLocation();
std::cout << "PLANNER: ROBOT LOCATION " << p.x << " " << p.y << " " << p.theta << std::endl;
switch (taskState) {
case START: // roll around to discover empty areas
if (!motionControl.rollStart(p))
taskState = EXPLORE;
break;
case EXPLORE: // seek unexplored areas until targets visible
if (ownTime_get_ms_since(hidasprkl) >= 5000) {
hidasprkl = ownTime_get_ms();
updateTargets();
}
if (targets.size()) {
if (motionControl.running())
motionControl.stop();
taskState = GO_TO_TARGET;
selectTarget();
navigateTarget();
} else {
if (!motionControl.iterate(p) && explRollStartTime == 0) {
explRollStartTime = ownTime_get_ms();
motionControl.setCtrl(0, 1.0 / 10 * M_PI);
} else if (explRollStartTime != 0 && ownTime_get_ms_since(explRollStartTime) > 3000) {
motionControl.setCtrl(0, 0);
SLAM::Location to(navigation.farthest());
explRollStartTime = 0;
std::cout << "PLANNER: explore to farthest " << to.x << " " << to.y << std::endl;
navigation.solveTo(to);
navigate();
//explore();
}
}
break;
case GO_TO_TARGET: // Move near the nearest target to open the hatch
if (motionControl.routeLeft() < HATCH_OPEN_DISTANCE) {
if (approachStarted < 1) {
approachStarted = ownTime_get_ms();
break;
}
camera.rotateNear();
if(ownTime_get_ms_since(approachStarted) < 1000) {
if (motionControl.running()) {
motionControl.stop();
}
break;
}
servoControl.setHatch(false);
navigateTarget();
approachStarted = 0;
taskState = APPROACH_PICK_UP;
}
motionControl.iterate(p);
break;
case APPROACH_PICK_UP: // Move on close to target
if (!motionControl.iterate(p)) {
float clear = navigation.wallClearance(p);
float walk = clear < PICKUPWALK ? clear : PICKUPWALK;
navigation.solveTo(SLAM::Location(p.x + cos(p.theta) * walk, p.y + sin(p.theta) * walk));
navigate();
taskState = PICK_UP;
}
break;
case GO_RETURN_TO_GOAL: // Try to find a route to the goal -- if cannot, explore more
ownSleep_ms(1000);
updateTargets();
if (navigation.solveTo(SLAM::Location(0, 0))) {
navigate();
taskState = RETURN_TO_GOAL;
} else {
taskState = EXPLORE;
}
break;
case RETURN_TO_GOAL: // Going to goal
if (!motionControl.iterate(p)) {
float clear = navigation.wallClearance(p);
float walk = clear < GOALWALK ? clear : GOALWALK;
navigation.solveTo(SLAM::Location(p.x + cos(p.theta) * walk, p.y + sin(p.theta) * walk));
navigate();
camera.rotateFar();
taskState = GOAL_WALKHAX;
}
break;
case GOAL_WALKHAX: // in goal, drive a bit forwards to be able to drop targets correctly
if (!motionControl.iterate(p)) {
taskState = RELEASE_TARGETS;
servoControl.setHatch(false);
}
break;
case PICK_UP: // when target is near, drive a bit forwards to be sure
if (!motionControl.iterate(p)) {
servoControl.setHatch(true);
camera.rotateFar();
numberOfPickUps++;
speak("Pallo " + lexical_cast(numberOfPickUps));
if(numberOfPickUps >= 10) {
taskState = GO_RETURN_TO_GOAL;
} else {
if (targets.empty()) {
taskState = EXPLORE;
} else {
selectTarget();
navigateTarget();
taskState = GO_TO_TARGET;
}
taskState = EXPLORE;
}
}
break;
case RELEASE_TARGETS: // eggs hatching, get back
servoControl.setHatch(false);
if (!motionControl.backFromGoal(p)) {
taskState = RETURN_TO_GOAL_AGAIN;
navigation.solveTo(SLAM::Location(0, 0));
navigate();
}
break;
case RETURN_TO_GOAL_AGAIN: // Going to goal another time to get last balls rolling
if (!motionControl.iterate(p)) {
float clear = navigation.wallClearance(p);
float walk = clear < GOALWALK_AGAIN ? clear : GOALWALK_AGAIN;
navigation.solveTo(SLAM::Location(p.x + cos(p.theta) * walk, p.y + sin(p.theta) * walk));
navigate();
camera.rotateFar();
taskState = GOAL_WALKHAX_AGAIN;
}
break;
case GOAL_WALKHAX_AGAIN: // in goal, drive a bit forwards to be able to drop targets correctly
if (!motionControl.iterate(p)) {
taskState = RELEASE_TARGETS_AGAIN;
}
break;
case RELEASE_TARGETS_AGAIN: // they see me rollin', they hatin'
if (!motionControl.backFromGoal(p)) {
taskState = END_STATE;
}
break;
case END_STATE: // world domination succeeded
camera.rotateNear();
speak("Hurraa");
manual.enabled = true;
break;
case BACK_OFF: // bumpers hit. exit to exploring when bumpers not hitting anymore
std::cout << "PLANNER: DYNDYNDYY" << std::endl;
motionControl.backOff();
break;
default: throw std::runtime_error("Bad task state number"); break;
}
float timeSince = ownTime_get_ms_since(statistics.taskStartTime);
if (timeSince >= hurryUp && (
taskState == START
|| taskState == EXPLORE
|| taskState == GO_TO_TARGET
|| taskState == APPROACH_PICK_UP
|| taskState == PICK_UP
)) {
taskState = GO_RETURN_TO_GOAL;
speak("Hop hop hop");
}
std::cout << "PLANNER: CURRENT TASK: " << taskdescr[taskState] << ", time elapsed " << (timeSince / 1000) << std::endl;
}
}
