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); }