void
ShowPosition(Loc x_loc, Loc y_loc, Direction tdir)
{
Rat myRat = M->rat(MY_RAT_INDEX);
clearPosition(MY_RAT_INDEX, myRat.x, myRat.y);
showMe(x_loc, y_loc, tdir);
}
Chess::Chess(char FEN[])
{
init_bitboard();
memset(moves, 0, STORE_SIZE);
clearPosition(&pos);
loadPositionFromFEN(FEN, &pos);
}
/*
* Someone left.
*/
void
ClearRatPosition(RatIndexType ratIndex)
{
Rat leftRat = M->rat(ratIndex);
clearPosition(ratIndex, leftRat.x, leftRat.y);
leftRat.playing = FALSE;
M->ratIs(leftRat, ratIndex);
}
/*
* Someone joined us; add him to the display.
*/
void
SetRatPosition(RatIndexType ratIndex, Loc x_loc, Loc y_loc, Direction dir)
{
Rat & newRat = M->updateRat(ratIndex);
newRat.playing = TRUE;
newRat.x = x_loc;
newRat.y = y_loc;
newRat.dir = dir;
// M->ratIs(newRat, ratIndex);
clearPosition(ratIndex, x_loc, y_loc);
}
void
ShowAllPositions()
{
RatIndexType ratIndex(0);
for (ratIndex = 0; ratIndex < MAX_RATS; ratIndex = RatIndexType(ratIndex.value() + 1)) {
if (ratIndex == MY_RAT_INDEX)
continue;
else if (M->rat(ratIndex).playing)
clearPosition(ratIndex, M->rat(ratIndex).x, M->rat(ratIndex).y);
}
}
void CachegrindLoader::prepareNewPart()
{
if (_part) {
// really new part needed?
if (mapping == 0) return;
// yes
_part->invalidate();
_part->totals()->clear();
_part->totals()->addCost(_part);
_data->addPart(_part);
partsAdded++;
}
clearCompression();
clearPosition();
_part = new TracePart(_data);
_part->setName(_filename);
}
static void initPos()
{
robotX=0;
robotY=0;
clearPosition();
}
//*****************************************************************************
//
//! Control two motor to make robot turn back 180 degree.
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of left motor.
//! \param NumPulse is the total pulse of two encoder after turn
//! \param resetEnc is the reset value for encoder after turning back
//! \return true if finish
//! false if not
//
static bool TurnBack(int fwdPulse, int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
LED1_ON();LED2_ON();LED3_ON();
switch (CtrlStep)
{
case 1:
{
posLeftTmp = qei_getPosLeft();
avrSpeedTmp = avrSpeed;
CtrlStep++;
}
case 2://go forward a litte bit
{
if (abs(qei_getPosLeft()-posLeftTmp)<fwdPulse)
{
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else if (isWallLeft)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_LEFT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
forwardUpdate();
CtrlStep++;
avrSpeed = avrSpeedTmp;
}
break;
}
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 4://turing 90 degree
{
#ifdef TEST_TURNBACK_FWD
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, avrSpeedLeft);
}
else
{
currentDir=(currentDir+3)%4;
CtrlStep++;
}
break;
}
case 5:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 6://turning another 90 degree
{
#ifdef TEST_TURNBACK_TURN1
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, -avrSpeedLeft);
speed_set(MOTOR_LEFT, -avrSpeedRight);
}
else
{
#ifdef TEST_TURNBACK_TURN2
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
return true;
}
break;
}
}
return false;
}
//*****************************************************************************
//
//! Control two motor to make robot turn left 90 degree
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of right motor.
//! \param turnPulse is the total pulse of two encoder after turn
//! \param resetEnc is reset value for encoder after turning 90 degree, ignore this if you don't want to estimate position
//! \return true if finish
//! false if not
//
//*****************************************************************************
static bool TurnLeft(int fwdPulse,int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
static int vt,vp;
LED1_ON();LED2_OFF();LED3_OFF();
// bluetooth_print("LS %d\r\n",CtrlStep);
switch (CtrlStep)
{
case 1:
posLeftTmp=qei_getPosLeft();
CtrlStep++;
avrSpeedTmp=avrSpeed;
case 2://go straight
if ((abs(qei_getPosLeft()-posLeftTmp)<fwdPulse) ||
(isWallFrontLeft && isWallFrontRight &&
(IR_GetIrDetectorValue(3)>IR_get_calib_value(IR_CALIB_BASE_FRONT_RIGHT))&&
(IR_GetIrDetectorValue(0)>IR_get_calib_value(IR_CALIB_BASE_FRONT_LEFT))))
{
if (qei_getPosLeft()<fwdPulse+posLeftTmp)
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
else
avrSpeed = (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE1
speed_Enable_Hbridge(false);
#endif
pid_reset(&pid_wall_right);
pid_reset(&pid_wall_left);
forwardUpdate();
CtrlStep++;
avrSpeed=avrSpeedTmp;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
vp=1;
vt=1;
case 4://turn 90 degree
if (abs(qei_getPosLeft()-posLeftTmp) + abs(qei_getPosRight()-posRightTmp) < turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, -avrSpeedLeft);
if((abs(qei_getPosRight()-posRightTmp)>(turnPulse*0.8*vp/8)) && (vp<9))
{
if (avrSpeedRight>=24)
avrSpeedRight-=24;
vp++;
}
if((abs(qei_getPosLeft()-posLeftTmp)>(turnPulse*0.2*vt/8)) && (vt<9))
{
if (avrSpeedLeft>=4)
avrSpeedLeft-=4;
vt++;
}
}
else
{
#ifdef TEST_TURNLEFT_TURN
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_LEFT, avrSpeed);
speed_set(MOTOR_RIGHT, avrSpeed);
return true;
}
break;
}
return false;
}
/// Clear the dataset, meaning remove all data
virtual void clear(void) throw()
{ clearPosition(); clearClock(); }
//generates a position given Forsyth-Edwards Notation
int loadFEN(Position* pos, char* fen){
int i, r, f;
char *pieces, *side, *castling, *enpassant, *halfmove, *fullmove;
char str[512];
Position newPos; //make our position locally, then copy it over at the end
strncpy(str, fen, 511);
pieces = strtok(str, " ");
side = strtok(NULL, " ");
if (!side) return 0;
castling = strtok(NULL, " ");
if (!castling) return 0;
enpassant = strtok(NULL, " ");
if (!enpassant) return 0;
halfmove = strtok(NULL, " ");
if (!halfmove) return 0;
fullmove = strtok(NULL, " ");
if (!fullmove) return 0;
clearPosition(&newPos);
newPos.ply = 2 * (atol(fullmove) - 1);
if (newPos.ply < 0) return 0;
if (strcmp(side, "b") == 0) {
newPos.ply++;
} else if (strcmp(side, "w") != 0) {
return 0;
}
newPos.castle = 0;
if (strcmp(castling, "-") != 0) {
if (*castling == 'K' || *castling == 'A') {
newPos.castle |= C_WK;
++castling;
}
if (*castling == 'Q' || *castling == 'H') {
newPos.castle |= C_WQ;
++castling;
}
if (*castling == 'k' || *castling == 'a') {
newPos.castle |= C_BK;
++castling;
}
if (*castling == 'q' || *castling == 'h') {
newPos.castle |= C_BQ;
++castling;
}
if (*castling) return 0;
}
if (strcmp(enpassant, "-") != 0) {
int file, rank;
file = enpassant[0] - 'a';
if (file < 0 || file >= 8) return 0;
rank = enpassant[1] - '1';
if (rank != 2 && rank != 5) return 0;
newPos.enpas = 8*rank + file;
}
newPos.ephash = EPHASH(&newPos);
for (i = 0, r = 7, f = 0; pieces[i]; ++i) {
int p;
char ch;
ch = pieces[i];
if (ch == '/'){
if (f != 8) return 0;
if (--r < 0) return 0;
f = 0;
} else if (ch > '0' && ch <= '8'){
f += (ch - '0');
} else {
if (f > 7) return 0;
p = getPiece(ch);
if (p == EMPTY) {
return 0;
}
newPos.bits[p] |= BIT(r*8 + f);
newPos.bits[OCCUPIED] |= BIT(r*8 + f);
newPos.bits[WHITE + ISBLACK[p]] |= BIT(r*8 + f);
newPos.score += VAL[p];
newPos.squares[r*8 + f] = p;
++f;
}
}
if (r != 0 && f != 8) return 0;
newPos.bits[EMPTY] = ~newPos.bits[OCCUPIED];
newPos.hash = hashPosition(&newPos);
*pos = newPos;
return 1;
}
void GFFLocation::clear() {
_area = kObjectIDInvalid;
clearPosition();
clearOrientation();
}
