//ForwardNode constructor
ForwardNode::ForwardNode(RobotState rs, int p, AbstractNode* parent) : AbstractNode(rs, prevCost, parent) {
//keep direction of previous RobotState
Direction dir = rs.getRobotDirection();
//update position dependent on direction
Position thisPos = rs.getRobotPosition();
switch (dir) {
case NORTH:
thisPos = Position(rs.getRobotPosition().x, rs.getRobotPosition().y + 1);
break;
case EAST:
thisPos = Position(rs.getRobotPosition().x + 1, rs.getRobotPosition().y);
break;
case SOUTH:
thisPos = Position(rs.getRobotPosition().x, rs.getRobotPosition().y - 1);
break;
case WEST:
thisPos = Position(rs.getRobotPosition().x - 1, rs.getRobotPosition().y);
break;
}
//assign thew new RobotState
curState = RobotState(dir, thisPos);
//Calculate the totalCost using prevCost + heuristic + travelCost
prevCost = p;
heuristic = World::getInstance().calculateHeuristic(curState.getRobotPosition());
travelCost = World::getInstance().getTerrain(curState.getRobotPosition());
offGrid = !World::getInstance().isInWorld(curState.getRobotPosition());
m_isGoal = World::getInstance().isGoal(curState.getRobotPosition());
if (m_isGoal)
travelCost -= 100;
totalCost = heuristic + prevCost + travelCost;
}
/**
* Constructor.
*/
Brain::Brain(BrainModules brain_modules, BrainConfig brain_config) : sensor_gap_min_dist_(brain_config.sensor_gap_min_dist),
desired_dist_to_wall_(brain_config.desired_dist_to_wall), front_sensor_stop_dist_(brain_config.front_sensor_stop_dist),
pixy_block_detection_threshold_(brain_config.pixy_block_detection_threshold), squaring_diff_threshold_(brain_config.squaring_diff_threshold)
{
visual_sensor_ = brain_modules.visual_sensor;
wall_sensors_ = brain_modules.wall_sensors;
motors_ = brain_modules.motors;
gyro_ = brain_modules.gyro;
front_detected_ = false;
last_heading_ = 0.0;
clearing_time_ = brain_config.clearing_time;
squaring_offset_ = brain_config.squaring_offset;
min_dist_to_wall_ = brain_config.min_dist_to_wall;
done_moving = false;
has_victim = false;
num_victims = 0;
victim_sig = 0;
robot_state_ = RobotState(brain_config.init_direction, brain_config.init_x, brain_config.init_y);
board_state_ = BoardState(brain_config.init_board_state);
}
/**
* Program: 1
* Competition run. Runs the robot from start to finish. Goes throughout the track to pick up all victims and returns to start.
* Returns true when it has returned to start.
*/
bool Robot::FinalRun()
{
auto GoToDropoffLocation = [this]() -> bool
{
ActionResult result;
if(brain_->victim_sig == 2) //yellow victim
{
result = brain_->GoToLocation(0, 1);
}
else //red victim
{
result = brain_->GoToLocation(7, 0);
}
switch(result)
{
case ACT_GOING:
return false;
break;
case ACT_SUCCESS://we're at dropoff location, return true
return true;
break;
default: //Error or fail stop code (shouldn't happen when going to first victim, so return true to stop robot)
return true;
break;
}
};
switch(brain_->num_victims)
{
case 0: //Pick up and drop off right city victim.
//If brain doesn't have the victim in its grasp
if(!brain_->has_victim)
{
//Go to victims location if we haven't already moved there
if(!brain_->done_moving)
{
switch(brain_->GoToLocation(7, 1))
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're on top of the victim; move to next step
brain_->done_moving = true;
break;
default: //Error or fail stop code (shouldn't happen when going to first victim, so return true to stop robot)
return true;
break;
}
}
else //we're done moving, pick up victim
{
//bite victim
if(motors_->BiteVictim())
{
//when done biting,
brain_->board_state_.RemoveVictim(7, 1); //remove victim from board state
brain_->robot_state_.SetOnVictim(false);
brain_->done_moving = false; //say we're not done moving
brain_->has_victim = true; //signal that we have a victim
}
}
}
else //robot has victim in its grasp
{
//Go to drop off location if we haven't already moved there
if(!brain_->done_moving)
{
if(GoToDropoffLocation())
{
brain_->done_moving = true;
}
}
else //we're done moving, drop off victim
{
if(brain_->DropOffVictim())
{
//when done biting, signal that we dropped off a victim
brain_->done_moving = false;
brain_->has_victim = false;
brain_->num_victims++; //now dropped off 1 more victim; go to next one
}
}
}
break;
case 1://Pick up and drop off left city victim
static bool checking_mexico = true;
static bool backup = true;
//instead of immediately going to the victim, we check the grass area to see if there is a victim
//and update the board state, then go to the left city victim.
if(checking_mexico)
{
//Go to scan location and face left side of board
if(!brain_->done_moving)
{
switch(brain_->GoToLocation(3, 3, LEFT))
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're at the scan location; move to next step
brain_->done_moving = true;
break;
default: //Error or fail stop code (shouldn't happen when going to second victim, so return true to stop robot)
return true;
break;
}
}
else //we're done moving, scan for the victim
{
//scan
switch(visual_sensor_->ScanForVictim())
{
case 0://results negative, victim not in the south; victim is up
brain_->board_state_.SetLeftVictimLocation(UP);
brain_->done_moving = false;
checking_mexico = false;
break;
case 1://results positive, victim in the south
brain_->board_state_.SetLeftVictimLocation(DOWN);
brain_->done_moving = false;
checking_mexico = false;
break;
case 2://still scanning
break;
}
}
}
else if(backup)
{
if(motors_->GoStraight(500000, 0, true))
{
motors_->StopPID();
backup = false;
}
}
//Done checking for victim, go to left city victim. If brain doesn't have the victim in its grasp
else if(!brain_->has_victim)
{
//Go to victims location if we haven't already moved there
if(!brain_->done_moving)
{
switch(brain_->GoToLocation(0, 2))
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're on top of the victim; move to next step
brain_->done_moving = true;
break;
default: //Error or fail stop code (shouldn't happen when going to second victim, so return true to stop robot)
return true;
break;
}
}
else //we're done moving, pick up victim
{
//bite victim
if(motors_->BiteVictim())
{
//when done biting,
brain_->board_state_.RemoveVictim(0, 2); //remove victim from board state
brain_->done_moving = false; //say we're not done moving
brain_->has_victim = true; //signal that we have a victim
}
}
}
else //robot has victim in its grasp
{
//Go to drop off location if we haven't already moved there
if(!brain_->done_moving)
{
if(GoToDropoffLocation())
{
brain_->done_moving = true;
}
}
else //we're done moving, drop off victim
{
if(brain_->DropOffVictim())
{
//when done biting, signal that we dropped off a victim
brain_->done_moving = false;
brain_->has_victim = false;
brain_->num_victims++; //now dropped off 1 more victim; go to next one
}
}
}
break;
case 2: //Go to right grass victim (to ensure we can get to the left victim if it is behind the river)
//If brain doesn't have the victim in its grasp
if(!brain_->has_victim)
{
static bool go_to_alt_location = false;
//Go to victims location if we haven't already moved there
if(!brain_->done_moving)
{
ActionResult result_flags = go_to_alt_location ? brain_->GoToLocation(5, 7) : brain_->GoToLocation(7, 5);
//Check if we've encountered a wall before we encounter the victim
//If true, this means we need to go to the alternate location instead.
if(result_flags & ACT_FRONT)
{
//Update our location on the board
brain_->robot_state_ = RobotState(UP, 7, 7);
//update the victim's location on the board.
brain_->board_state_.SetRightVictimLocation(UP);
//signal that next loop we go to the alternate path
go_to_alt_location = true;
return false;
}
switch(result_flags)
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're on top of the victim; move to next step
//If we got here without using alternate path, update the victim's location on the board
if(!go_to_alt_location) brain_->board_state_.SetRightVictimLocation(DOWN);
brain_->done_moving = true;
break;
default: //Error or fail stop code (shouldn't happen when going to first victim, so return true to stop robot)
return true;
break;
}
}
else //we're done moving, pick up victim
{
//bite victim
if(motors_->BiteVictim())
{
//when done biting,
if(go_to_alt_location)
{
brain_->board_state_.RemoveVictim(5, 7); //remove victim from board state
}
else
{
brain_->board_state_.RemoveVictim(7, 5); //remove victim from board state
}
brain_->done_moving = false; //say we're not done moving
brain_->has_victim = true; //signal that we have a victim
}
}
}
else //robot has victim in its grasp
{
//Go to drop off location if we haven't already moved there
if(!brain_->done_moving)
{
if(GoToDropoffLocation())
{
brain_->done_moving = true;
}
}
else //we're done moving, drop off victim
{
if(brain_->DropOffVictim())
{
//when done biting, signal that we dropped off a victim
brain_->done_moving = false;
brain_->has_victim = false;
brain_->num_victims++; //now dropped off 1 more victim; go to next one
}
}
}
break;
case 3://Go to left grass victim
//If brain doesn't have the victim in its grasp
if(!brain_->has_victim)
{
bool go_to_mexico = brain_->board_state_.HasVictim(0, 3);
//Go to victims location if we haven't already moved there
if(!brain_->done_moving)
{
ActionResult result = go_to_mexico ? brain_->GoToLocation(0, 3) : brain_->GoToLocation(0, 5);
switch(result)
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're on top of the victim; move to next step
brain_->done_moving = true;
break;
default: //Error or fail stop code (shouldn't happen when going to first victim, so return true to stop robot)
return true;
break;
}
}
else //we're done moving, pick up victim
{
//bite victim
if(motors_->BiteVictim())
{
//when done biting,
if(go_to_mexico)
{
brain_->board_state_.RemoveVictim(0, 3); //remove victim from board state
}
else
{
brain_->board_state_.RemoveVictim(0, 5); //remove victim from board state
}
brain_->done_moving = false; //say we're not done moving
brain_->has_victim = true; //signal that we have a victim
}
}
}
else //robot has victim in its grasp
{
//Go to drop off location if we haven't already moved there
if(!brain_->done_moving)
{
if(GoToDropoffLocation())
{
brain_->done_moving = true;
}
}
else //we're done moving, drop off victim
{
if(brain_->DropOffVictim())
{
//when done biting, signal that we dropped off a victim
brain_->done_moving = false;
brain_->has_victim = false;
brain_->num_victims++; //now dropped off 1 more victim; go to next one
}
}
}
break;
case 4://We've deposited all 4 victims, return to start
switch(brain_->GoToLocation(0, 0))
{
case ACT_GOING: // do nothing
break;
case ACT_SUCCESS://we're back at start, return true
return true;
break;
}
break;
}
return false;
}
