// --------------------------------------------------------------------------
// Quand on est pret du pont, le robot tourne sur la droite pour voir s'il y
// a vraiment un pont ou non, mais il ne met qu'une roue dans le trou! Ca
// evite de tomber
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryRotateToSeeBridge()
{
bool result=true;
if ((bridge_ == BRIDGE_POS_BORDURE)
|| (!bridgeDetectionByCenter_
&& bridge_ == BRIDGE_ENTRY_CENTER_Y)) {
// il faut faire une rotation sur une roue pour voir si le pont
// et la et eviter de tomber comme une merde
LOG_COMMAND("Rotate to detect Bridge\n");
enableBridgeCaptors();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->enableAccelerationController(true);
Move->rotateOnWheel(-M_PI_4/1.8 , false, -1, 15);
bool dummyBumperEvt=true;
while(dummyBumperEvt) {
Events->wait(evtEndMoveBridge);
Move->enableAccelerationController(false);
if (checkBridgeBumperEvent(dummyBumperEvt)) {
result = false;
break;
}
}
if (result && bridge_ == BRIDGE_POS_BORDURE) useBridgeBumpers_=false; // on sait que le pont est la
if (checkEndEvents())
return false;
disableBridgeCaptors();
Move->rotateOnWheel(0 , false, 2, 30);
Events->wait(evtEndMove);
if (checkEndEvents())
return false;
}
return result;
}
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont juste un peu decalle en y (~15cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryNear(Millimeter y)
{
Trajectory t;
disableBridgeCaptors();
LOG_COMMAND("gotoBridgeEntry Hard Near:%d\n", (int)y);
// on ne va pas loin en y donc on fait un joli mouvement en S
// met les servos en position
Millimeter y2 = RobotPos->y();
if (y>1800) { // pont du bord
Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
} else if (y<1500) { // pont du bord ou pont contre le milieu
Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
} else { // ponts au milieu
t.push_back(Point(BRIDGE_DETECT_BUMP_X-100, y2));
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, (2*y+y2)/3));
Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 3, 30);
}
Events->wait(evtEndMove);
Move->enableAccelerationController(false);
if (checkEndEvents()) return false;
if(!Events->isInWaitResult(EVENTS_MOVE_END)) {
// collision
Move->backward(100);
Events->wait(evtEndMoveNoCollision);
return false;
}
// va en face du pont
enableBridgeCaptors();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(BRIDGE_DETECT_BUMP_X, y, 2, 30);
// dont need to wait, it is done by the upper function
return true;
}
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont tres decalle en y (>20cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryFar(Millimeter y)
{
disableBridgeCaptors();
Millimeter y2 = RobotPos->y();
LOG_COMMAND("gotoBridgeEntry Hard Far:%d\n", (int)y);
// on va loin: on recule bettement puit on prend un point cible
// on s'eloigne un peu du bord, pour aller en x qui nous permet
//de nous promener tranquillement le long de la riviere
Move->go2Target(BRIDGE_ENTRY_NAVIGATE_X, y2, 2, 40); // norotation
Events->wait(evtEndMove);
if (checkEndEvents() ||
!Events->isInWaitResult(EVENTS_MOVE_END)) {
Move->enableAccelerationController(false);
return false;
}
// va en face du pont
enableBridgeCaptors();
Trajectory t;
// Pour eviter les rotations finales on
if (y2 < y)
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y-75));
else
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y+75));
t.push_back(Point(BRIDGE_DETECT_BUMP_X, y));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 2, 30, true); // norotation
// dont wait events, it is done by the upper function
return true;
}
common::Error SsdbDriver::serverInfo(IServerInfo **info) {
LOG_COMMAND(Command(INFO_REQUEST, common::Value::C_INNER));
SsdbServerInfo::Common cm;
common::Error err = impl_->info(nullptr, &cm);
if (!err) {
*info = new SsdbServerInfo(cm);
}
return err;
}
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont en face du robot
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryEasy(Millimeter y)
{
if (RobotPos->x() > BRIDGE_DETECT_BUMP_X - 30) return true;
enableBridgeCaptors();
LOG_COMMAND("gotoBridgeEntry Easy:%d\n", (int)y);
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(Point(BRIDGE_DETECT_BUMP_X, y),
ATTACK_BRIDGE_GAIN, ATTACK_BRIDGE_SPEED);
return true;
}
common::Error Driver::CurrentServerInfo(IServerInfo** info) {
FastoObjectCommandIPtr cmd = CreateCommandFast(MEMCACHED_INFO_REQUEST, common::Value::C_INNER);
LOG_COMMAND(cmd);
ServerInfo::Stats cm;
common::Error err = impl_->Info(nullptr, &cm);
if (err && err->isError()) {
return err;
}
*info = new ServerInfo(cm);
return common::Error();
}
void Driver::HandleLoadDatabaseContentEvent(events::LoadDatabaseContentRequestEvent* ev) {
QObject* sender = ev->sender();
NotifyProgress(sender, 0);
events::LoadDatabaseContentResponceEvent::value_type res(ev->value());
std::string patternResult = common::MemSPrintf(SSDB_GET_KEYS_PATTERN_1ARGS_I, res.count_keys);
FastoObjectCommandIPtr cmd = CreateCommandFast(patternResult, common::Value::C_INNER);
NotifyProgress(sender, 50);
common::Error err = Execute(cmd);
if (err && err->isError()) {
res.setErrorInfo(err);
} else {
FastoObject::childs_t rchildrens = cmd->Childrens();
if (rchildrens.size()) {
CHECK_EQ(rchildrens.size(), 1);
FastoObjectArray* array = dynamic_cast<FastoObjectArray*>(rchildrens[0].get()); // +
if (!array) {
goto done;
}
common::ArrayValue* ar = array->Array();
if (!ar) {
goto done;
}
for (size_t i = 0; i < ar->size(); ++i) {
std::string key;
if (ar->getString(i, &key)) {
NKey k(key);
FastoObjectCommandIPtr cmd_ttl =
CreateCommandFast(common::MemSPrintf("TTL %s", key), common::Value::C_INNER);
LOG_COMMAND(cmd_ttl);
ttl_t ttl = NO_TTL;
common::Error err = impl_->TTL(key, &ttl);
if (err && err->isError()) {
k.SetTTL(NO_TTL);
} else {
k.SetTTL(ttl);
}
NValue empty_val(common::Value::createEmptyValueFromType(common::Value::TYPE_STRING));
NDbKValue ress(k, empty_val);
res.keys.push_back(ress);
}
}
err = impl_->DBkcount(&res.db_keys_count);
DCHECK(!err);
}
}
done:
NotifyProgress(sender, 75);
Reply(sender, new events::LoadDatabaseContentResponceEvent(this, res));
NotifyProgress(sender, 100);
}
// --------------------------------------------------------------------------
// verifie que tous les capteurs pont disent qu'il y a un pont en position
// de depart. Si ce n'est pas le cas: si c'est un sharp qui merde, on
// desactive la detection sharp, si c'est un bumper un demande de reessayer,
// si l'utilisateur skip, on desactive le capteur => risque de finir dans le
// trou
// --------------------------------------------------------------------------
bool StrategyAttackCL::testBridgeCaptors()
{
char txt[64];
LOG_COMMAND("== testBridgeCaptors ==\n");
BridgeCaptorStatus captors[BRIDGE_CAPTORS_NBR];
while(true) {
// lit la valeur des capteurs sur la carte bumper
while(true) {
if (Bumper->getBridgeCaptors(captors)) break;
LOG_ERROR("Cannot get bridge informations from bumpers\n");
if (!menu("getBridge error\nRetry Skip")) break;
}
for(int i=0;i<BRIDGE_CAPTORS_NBR;i++) {
sprintf(txt, "%s[%d.1]=%s, ", txt,
i, captors[i]==BRIDGE_NO?"trou":"pont");
}
// teste les sharps
if (useSharpToDetectBridge_) {
if (bridgeDetectionByCenter_) {
if (captors[BRIDGE_SHARP_LEFT] == BRIDGE_NO) {
LOG_ERROR("bridge sharp left does not work => disable bridge "
"sharps!\n");
//useSharpToDetectBridge_ = false;
}
} else {
if (captors[BRIDGE_SHARP_LEFT] == BRIDGE_NO ||
captors[BRIDGE_SHARP_CENTER] == BRIDGE_NO ||
captors[BRIDGE_SHARP_RIGHT] == BRIDGE_NO) {
LOG_ERROR("At least one bridge sharp(%s) does not work => "
"disable bridge sharps!\n", BridgePosTxt(bridge_));
//useSharpToDetectBridge_ = false;
}
}
}
// teste les bumpers
if (captors[BRIDGE_BUMPER_LEFT] == BRIDGE_NO) {
LOG_ERROR("Left bridge bumper does not work\n");
if (menu("Bump Bridge left\nErr Retry Skip")) continue;
else Bumper->disableCaptor(BRIDG_BUMP_LEFT);
}
if (captors[BRIDGE_BUMPER_RIGHT] == BRIDGE_NO) {
LOG_ERROR("Right bridge bumper does not work\n");
if (menu("Bump Bridge right\nErr Retry Skip")) continue;
else Bumper->disableCaptor(BRIDG_BUMP_RIGHT);
}
return true;
}
return true;
}
// ---------------------------------------------------------------
// strategie exploration en passant par la rangee en y = 1350
// ---------------------------------------------------------------
bool StrategyLargeAttackCL::preDefinedSkittleExploration2()
{
LOG_COMMAND("preDefinedLargeSkittleExploration2\n");
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
bool noRotation = true;
Point lineStart(2500, 1500);
Point lineEnd(3200, 1500);
Move->go2Target(lineStart,
MOVE_USE_DEFAULT_GAIN,
MOVE_USE_DEFAULT_SPEED,
noRotation);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
if (checkEndEvents()) return false; // c'est la fin du match?
CollisionEnum collision = checkCollisionEvents();
if (collision != COLLISION_NONE) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else {
LOG_WARNING("don't know what caused abort of movement. -> abort predefined exploration\n");
return false;
}
} else {
// go2Target succeeded its movement.
Move->rotate(0); // face right border
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
if (checkEndEvents()) return false; // end of match
// ok. normally the collision can only be on the left side...
CollisionEnum collision = checkCollisionEvents();
if (collision == COLLISION_LEFT) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else if (collision == COLLISION_NONE) {
LOG_WARNING("unhandled event. leaving function\n");
return false;
} else {
LOG_WARNING("collision, but most likely not a support\n");
return false;
}
}
}
bool endOfLine = false;
while (!endOfLine) {
Move->go2Target(lineEnd);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// let's hope it's a support.
if (checkEndEvents()) return false; // c'est la fin du match?
CollisionEnum collision = checkCollisionEvents();
if (collision != COLLISION_NONE) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else {
LOG_WARNING("don't know what caused abort of movement. -> abort predefined exploration\n");
return false;
}
if (!RobotPos->isTargetForward(lineEnd))
endOfLine = true;
} else {
// ok. no more supports detected
endOfLine = true;
}
}
LOG_INFO("predefined-large finished.\n");
//alignBorder();
// on recule un petit peu car on ne sais pas ce qu'on va se prendre en
// approchant du robot adverse!, mieux vaut tenir que courir
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Trajectory t;
t.clear();
t.push_back(Point(3144, 1350));
t.push_back(Point(3190, 1700));
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
// on va droit sur l'adversaire
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(Point(3190, 650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
// on recule un peu
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1050));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
t.clear();
t.push_back(Point(2550, 1050));
t.push_back(Point(2594, 1650));
t.push_back(Point(3340, 1650));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
return true;
}
// ---------------------------------------------------------------
// strategie exploration en passant par la rangee en y = 1650
// ---------------------------------------------------------------
bool StrategyLargeAttackCL::preDefinedSkittleExploration1()
{
return preDefinedSkittleExploration2();
LOG_COMMAND("preDefinedLargeSkittleExploration1\n");
Trajectory t;
t.push_back(Point(2640, 1650));
t.push_back(Point(3240, 1650));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->rotate(-M_PI_2);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(Point(3190, 650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1050));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
t.clear();
t.push_back(Point(2550, 1050));
t.push_back(Point(2594, 1400));
t.push_back(Point(3340, 1350));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1350));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
return true;
}
// --------------------------------------------------------------------------
// quand on est aligne avec un pont et situe a son entree, on part de l'autre
// cote du pont. Si les bumpers de pont disent qu'il y a un trou, on recule.
// --------------------------------------------------------------------------
bool StrategyAttackCL::crossBridge()
{
Point tgt(BRIDGE_CROSS_BRIDGE_X, RobotPos->y());
if (bridgeDetectionByCenter_) {
if (bridge_ == BRIDGE_POS_CENTER ||
bridge_ == BRIDGE_POS_MIDDLE_CENTER) {
tgt.y=BRIDGE_ENTRY_SIOUX_Y;
} else if (bridge_ == BRIDGE_POS_MIDDLE_BORDURE) {
tgt.y=BRIDGE_ENTRY_MIDDLE_BORDURE_Y;
} else if (bridge_ == BRIDGE_POS_BORDURE) {
tgt.y=BRIDGE_ENTRY_BORDURE_Y;
}
} else {
if (bridge_ == BRIDGE_POS_CENTER) {
tgt.y=BRIDGE_ENTRY_CENTER_Y;
} else if (bridge_ == BRIDGE_POS_MIDDLE_CENTER) {
tgt.y=BRIDGE_ENTRY_MIDDLE_CENTER_Y;
} else if (bridge_ == BRIDGE_POS_MIDDLE_BORDURE) {
tgt.y=BRIDGE_ENTRY_MIDDLE_BORDURE_Y;
} else if (bridge_ == BRIDGE_POS_BORDURE) {
tgt.y=BRIDGE_ENTRY_BORDURE_Y;
}
}
if (!useLeftBridge_) {
tgt.y = TERRAIN_Y - tgt.y;
}
// verifie qu'on est bien aligne
if (fabs(tgt.y-RobotPos->y()) > BRIDGE_ENTRY_MARGIN)
return false;
int retry=0;
LOG_COMMAND("crossBridge: %s\n", tgt.txt());
Move->enableAccelerationController(true);
do {
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(tgt);
enableBridgeCaptors();
bool dummyBumperEvt=true;
while(dummyBumperEvt) {
Events->wait(evtEndMoveBridge);
Move->enableAccelerationController(false);
if (checkBridgeBumperEvent(dummyBumperEvt)) {
if (RobotPos->x()> 1600) {
useBridgeBumpers_=false;
disableBridgeCaptors();
dummyBumperEvt = true;
continue;
} else {
Move->backward(100);
Events->wait(evtEndMoveNoCollision);
return false;
}
} else if (dummyBumperEvt) {
Move->enableAccelerationController(true);
Move->go2Target(tgt);
enableBridgeCaptors();
}
}
if (RobotPos->x()> 1600) {
useBridgeBumpers_=false;
disableBridgeCaptors();
}
// on a reussi a traverser?
if (Events->isInWaitResult(EVENTS_MOVE_END)) return true;
// c'est la fin du match?
if (checkEndEvents()) return false;
// on s'est plante, on recule un peu, on se reoriente et on
// repart!
Move->realign(0);
Events->wait(evtEndMoveNoCollision);
if (checkEndEvents()) return false;
Move->backward(100);
// Events->wait(evtEndMoveNoCollision);
// if (checkEndEvents()) return false;
// Move->rotate(0);
Events->wait(evtEndMoveNoCollision);
if (checkEndEvents()) return false;
} while (retry++<5);
// on n'a pas reussi a traverser par ici... on recule et on essaye ailleurs
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
tgt.x=BRIDGE_DETECT_SHARP_X;
Move->go2Target(tgt);
Events->wait(evtEndMoveNoCollision);
return false;
}
