void pid_Wallfollow_process(void)
{
if (ControlFlag)
{
static int i;
pid_Runtimeout(&pid_process_callback, ui32_msLoop);
ControlFlag = false;
leftError=(float)IR_get_calib_value(IR_CALIB_BASE_LEFT) - (float)IR_GetIrDetectorValue(1);
rightError=(float)IR_get_calib_value(IR_CALIB_BASE_RIGHT) - (float)IR_GetIrDetectorValue(2);
isWallLeft = IR_GetIrDetectorValue(1)<IR_get_calib_value(IR_CALIB_MAX_LEFT);
isWallRight = IR_GetIrDetectorValue(2)<IR_get_calib_value(IR_CALIB_MAX_RIGHT);
isWallFrontLeft = IR_GetIrDetectorValue(0)<IR_get_calib_value(IR_CALIB_MAX_FRONT_LEFT);
isWallFrontRight = IR_GetIrDetectorValue(3)<IR_get_calib_value(IR_CALIB_MAX_FRONT_RIGHT);
switch(eMove)
{
case FORWARD:
switch (moveStage)
{
case 1:
if (Forward())
moveStage++;
if (isWallFrontLeft| isWallFrontRight)
{
preMove=eMove;
eMove=getMove(isWallLeft,isWallFrontLeft|isWallFrontRight,isWallRight);
}
break;
case 2:
posLeftTmp=qei_getPosLeft();
moveStage++;
i=1;
avrSpeedTmp=avrSpeed;
case 3://slow down
forwardUpdate();
if (!isWallLeft)
{
rqTurnLeft=true;
}
if (!isWallRight)
{
rqTurnRight=true;
}
if ((abs(qei_getPosLeft()-posLeftTmp)<5000)
&& (!isWallFrontLeft) && (!isWallFrontRight))
{
if ((abs(qei_getPosLeft()-posLeftTmp)>i*500) && (avrSpeed>AVG_SPEED_FWD-40))
{
avrSpeed -= 10;
i++;
}
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_FORWARD_MOVE
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
if (eMove==FORWARD)
avrSpeed=AVG_SPEED_FWD;
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
}
break;
}
break;
case TURN_LEFT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ___
// |__ |
// |__|
fwdPulse=5500;
else//after moving forward
//test
// ___
// ___ |
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnLeft(fwdPulse,60,240,7800,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
//go straight a little bit to check wall
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<2000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>1500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);//left motor is faster
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE3
speed_Enable_Hbridge(false);
#endif
//time to check front wall
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_RIGHT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ____
// | ____
// |__|
fwdPulse=5500;
else//after moving forward
//test
// _____
// | _____
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnRight(fwdPulse,200,40,8000,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<1000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNRIGHT_MOVE3
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_BACK:
switch (moveStage)
{
case 1:
if (preMove==FORWARD)
fwdPulse=8000;
else
fwdPulse=9000;
moveStage++;
case 2:
if (TurnBack(fwdPulse,-140,60,8000,13000))
{
//rotate more if we still detect front wall: do it yourself ;D
moveStage++;
}
break;
case 3:
if (move(-9000,-9000,AVG_SPEED_BWD,AVG_SPEED_BWD))
{
#ifdef TEST_TURNBACK_BACKWARD
speed_Enable_Hbridge(false);
#endif
forwardUpdate();
avrSpeed = AVG_SPEED_FWD_SLOW;
preMove=eMove;
eMove=FORWARD;
moveStage = 1;
}
}
break;
}
}
}
void Player::Update(const Ogre::FrameEvent& evt,GameMap* gameMap)
{
Ogre::Vector3 transVector = Ogre::Vector3::ZERO;
//get input
if (_Keyboard->isKeyDown(OIS::KC_SPACE))
{
if (!_playerIsPlacingBomb)
{
if (_bombAvailableNumber > 0)
{
GameBomb* bomb = new GameBomb(_Entity,_Node,_SceneMgr,_Camera,_Keyboard,_bombLevel);
//switch(_direction)
//{
//case LEFT_DIRECTION:
// gameMap->setMapTypeAtGridPos(convertWorldPosToGridPos(_Node->getPosition()).x+1,convertWorldPosToGridPos(_Node->getPosition()).y,GRID_BOMB);
// bomb->setPosition(Ogre::Vector2(convertWorldPosToGridPos(_Node->getPosition()).x+1,convertWorldPosToGridPos(_Node->getPosition()).y));
// break;
//case RIGHT_DIRECTION:
// gameMap->setMapTypeAtGridPos(convertWorldPosToGridPos(_Node->getPosition()).x-1,convertWorldPosToGridPos(_Node->getPosition()).y,GRID_BOMB);
// bomb->setPosition(Ogre::Vector2(convertWorldPosToGridPos(_Node->getPosition()).x-1,convertWorldPosToGridPos(_Node->getPosition()).y));
// break;
//case UP_DIRECTION:
// gameMap->setMapTypeAtGridPos(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y+1,GRID_BOMB);
// bomb->setPosition(Ogre::Vector2(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y+1));
// break;
//case DOWN_DIRECTION:
// gameMap->setMapTypeAtGridPos(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y-1,GRID_BOMB);
// bomb->setPosition(Ogre::Vector2(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y-1));
// break;
//default:
// break;
//}
gameMap->setMapTypeAtGridPos(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y,GRID_BOMB);
bomb->setPosition(Ogre::Vector2(convertWorldPosToGridPos(_Node->getPosition()).x,convertWorldPosToGridPos(_Node->getPosition()).y));
_bombList.insert(std::pair<int,GameBomb*>(_bombIndex++,bomb));
bomb->bombIndex = _bombIndex;
_bombAvailableNumber--;
}
_playerIsPlacingBomb = true;
}
}
else
{
_playerIsPlacingBomb = false;
}
if (_Keyboard->isKeyDown(OIS::KC_RIGHT))
{
playerMoveRight();
transVector.x += _leftToRight * _moveSpeed;
transVector.x -= _rightToUp * _moveSpeed;
transVector.x -= _rightToDown * _moveSpeed;
}
else if (_Keyboard->isKeyDown(OIS::KC_LEFT))
{
playerMoveLeft();
transVector.x -= _leftToUp * _moveSpeed;
transVector.x -= _leftToRight * _moveSpeed;
transVector.x -= _leftToDown * _moveSpeed;
}
else if (_Keyboard->isKeyDown(OIS::KC_UP))
{
playerMoveUp();
transVector.x += _leftToUp * _moveSpeed;
transVector.x += _rightToUp * _moveSpeed;
transVector.x -= _upToDown * _moveSpeed;
}
else if (_Keyboard->isKeyDown(OIS::KC_DOWN))
{
playerMoveDown();
transVector.x += _leftToDown * _moveSpeed;
transVector.x += _rightToDown * _moveSpeed;
transVector.x += _upToDown * _moveSpeed;
}
//update
updateAnimation(evt);
transVector=transVector *evt.timeSinceLastFrame;
transVector.x=(int)transVector.x;
transVector.y=(int)transVector.y;
transVector.z=(int)transVector.z;
this->_Node->translate(transVector,Ogre::Node::TS_LOCAL);
//this->_Node->translate(transVector ,Ogre::Node::TS_LOCAL);
this->_pos = convertWorldPosToGridPos(this->_Node->getPosition());
updatePlayerWithGrid(evt,gameMap);
updateBomb(evt,gameMap);
TurnBack(gameMap);
}
