void setEnvironment() {
checkTerminal();
saveAttributes();
setMenuMode();
registerSigIntCatcher();
disableRepeatAndCursor();
}
void GrammarParser::checkGrammar() {
for(int i = 1; i < m_grammar.getSymbolCount(); i++) { // dont check EOI
const GrammarSymbol &sym = m_grammar.getSymbol(i);
if(sym.m_type == NONTERMINAL) {
checkNonTerminal(i);
} else {
checkTerminal(i);
}
}
if(m_grammar.getProductionCount() == 0) {
m_lex.error(_T("No productions in grammar."));
} else {
checkDuplicateProd();
checkReachability();
checkTermination();
}
}
void GoalLineUp::setState(const State& s, bool& terminal)
{
CurrentState = s;
if ((CurrentState.x[0]<0)||(CurrentState.x[0]>100))
{
cout << "State set to invalid value" << endl;
cout << s << endl;
exit(EXIT_FAILURE);
}
if ((CurrentState.x[1] < -25)||(CurrentState.x[1] > 25)){
cout << "State set to invalid value" << endl;
cout << s << endl;
exit(EXIT_FAILURE);
}
terminal = checkTerminal();
Stages=1;
}
void GoalLineUp::makeObservation(double &reward, bool initial)
{
static bool firstCall = true;
measureddistance = 0;
balldistance = 0;
ballbearing = 0;
compass = 0;
mixedSpeed = 0;
Blackboard->Sensors->getCompass(compass );
currPos[2] = compass;
Blackboard->Sensors->getBallGps(ballPos);
if(firstCall)
{
firstCall = false;
oldBallPos = ballPos;
}
//cout<<"\nBall position : ["<<ballPos[0]<<", "<<ballPos[1]<<" ]";
bool retVal = false;
MobileObject& ball = Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL];
measureddistance = Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL].measuredDistance();
balldistance = measureddistance * cos(Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL].measuredElevation());
ballbearing = Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL].measuredBearing();
kickPosition[0] = ball.estimatedDistance() * cos(ball.estimatedBearing());
kickPosition[1] = ball.estimatedDistance() * sin(ball.estimatedBearing());
targetPosition[0] = kickPosition[0] + 1000.0f;
targetPosition[1] = kickPosition[1];
//Blackboard->Sensors->get(NUSensorsData::MotionWalkSpeed,currVel);
//mixedSpeed = (transVel + rotVel)/2;
if(ball.estimatedDistance() >= 120)
CurrentState.x[0] = 120;
else
CurrentState.x[0] = ball.estimatedDistance();
double currAngle = mathGeneral::rad2deg(currPos[2]);;
if(currAngle < 0 )
currAngle = 180 + currAngle;
else
currAngle = currAngle - 180;
CurrentState.x[1] = currAngle;
CurrentState.x[2] = transVel;
CurrentState.x[3] = rotVel;
if (Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL].isObjectVisible())
{
CurrentState.x[4] = 1;
if(blindFrameCount>0)
blindFrameCount--;
}
else
{
CurrentState.x[0] = s_last.x[0]; // if ball is not visible, donot decrease its distance from robot
CurrentState.x[4] = 0;
cout<<"\nI cannot see the ball!";
}
//cout<<"\nBall position : ["<<ballPos[0]<<", "<<ballPos[1]<<" ]";
static int moved = 0;
int movedX = fabs(BALLINITIAL_X - ballPos[0]);
int movedY = fabs( BALLINITIAL_Y - ballPos[1]);
//cout<<"\nBall moved condition : "<<movedX <<", "<<movedY<<"\n";
if(!initial)
{
if(checkTerminal() && Stages>2 )
{
reward = 100;
//terminal = true;
cout<<"\n\n\nRewarding +ve heavily !\n\n\n\n\n\n";
Blackboard->Actions->restartCondition = true;
}
if ( (movedX>2 || movedY>2))
{
moved++;
if (moved>2)
{
cout<<"\nBall moved, restarting with negative reward!";
reward = -10;
// Blackboard->Actions->restartCondition = true;
}
}
vector<float>roboPose(3,0);
Blackboard->Sensors->getGps(roboPose);
Blackboard->Sensors->getBallGps(ballPos);
double balldistanceX = roboPose[0] - ballPos[0];
double balldistanceY = roboPose[1] - ballPos[1];
// cannot see the ball
if(CurrentState.x[4]==0) // || !(Blackboard->Objects->mobileFieldObjects[FieldObjects::FO_BALL].isObjectVisible()))
{
reward = -10;
cout<<"\nReturning heavy -ve reward!\n\n";
}
/* else if ( balldistanceX < 25 && (transVel>0.25 || rotVel>0.25) ) //// if reached ball but still walking
{
cout<<"\nReturning heavy -ve reward!\n\n";
reward = -10;
Blackboard->Actions->restartCondition = true;
}
*/
else
reward = -1;
}
}
