Colony » Colony Scout

set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/line_follow.cpp src/behaviors/wl_test.cpp src/behaviors/pause_scout.cpp)

set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp src/behaviors/maze_solve.cpp)

#include "BehaviorList.h"

  behavior_list.push_back(behavior<draw_cw_circle>);

  behavior_list.push_back(behavior<draw_ccw_circle>);

  behavior_list.push_back(behavior<line_follow>);

  behavior_list.push_back(behavior<wl_test>);

#include "behaviors/line_follow.h"

#include "behaviors/draw_cw_circle.h"

#include "behaviors/draw_ccw_circle.h"

#include "behaviors/Scheduler.h"

#include "behaviors/WH_Robot.h"

#include "behaviors/pause_scout.h"

#include "behaviors/maze_solve.h"

#include "Sensors.h"

#include "Scheduler.h"

#include "../helper_classes/Order.h"

using namespace std;

/** @Brief: Initialize data structures for the scheduler. */

Scheduler::Scheduler(std::string scoutname, Sensors* sensors):

            Behavior(scoutname, "Scheduler", sensors)

{

  unassignedOrders = new PQWrapper(NUM_TASKS);

	create_orders();

}

/** @Brief: Free allocatetd memory. */

Scheduler::~Scheduler()

{

  delete unassignedOrders;

}

/** @Brief: Add robot to the waiting queue. 

 *  A robot calls this function with itself

 *  and gets pushed on a list of waiting robots.

 *  When a task is availaible the scheduler, removes

 *  the robot of the waiting queue, and gives it a

 *  task.

 */

void Scheduler::get_task(int robot)

{

  Robot my_robot = robots[robot-1];

  if(my_robot.sched_status != WAITING_ROBOT)

  {

	  waitingRobots.push(my_robot);

    my_robot.sched_status = WAITING_ROBOT;

  }

}

/** @Brief: Statically add orders to the Priority Queue Wrapper.*/

void Scheduler::create_orders()

{

  Path p;

  p.len = 0;

  p.path = NULL;

  Time t = ros::Time::now();

  Duration end(0);

  Duration five(5);

  Duration three(3);

  Duration ten(10);

	Order a(1,0,13,t+ten,p,end);

  Order b(2,0,14,t+five,p,end);

  Order c(3,0,15,t+three,p,end);

  Order d(4,0,16,t,p,end);

	unassignedOrders->insert(a);

	unassignedOrders->insert(b);

	unassignedOrders->insert(c);

	unassignedOrders->insert(d);

}

/** @Brief: This is a confirmation that the task is complete. 

    This function removes the order from assignedOrders. */

void Scheduler::task_complete(Order o)

{

  ROS_INFO("Scheduler: Task id %d was completed", o.getid());

  for (unsigned int i=0; i<assignedOrders.size(); i++)

  {

    if (assignedOrders[i].getid()==o.getid())

    {

      assignedOrders.erase(assignedOrders.begin()+i);

    }

  }

}

/** @Brief: This is a confirmation that the task failed. 

    This function places the order back on the PQWrapper. */

void Scheduler::task_failed(Order o)

{

  ROS_INFO("Scheduler: Task id %d was failed", o.getid());

	task_complete(o);

	unassignedOrders->insert(o);

}

/** @Brief: Do a waiting dance. */

void Scheduler::waiting_dance()

{ 

    //ROS_INFO("Scheduler: TEEHEE i do a dance!");

}

/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */

Order Scheduler::get_next_item()

{

  Time t = ros::TIME_MAX; 

  double time = t.toSec();

  Order* best;

  int best_index;

  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)

  {

    Order order = unassignedOrders->peek(i);

    Time end_time = order.get_start_time() - order.get_est_time();

    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function

    {

      best = ℴ

      best_index = i;

      time = end_time.toSec() + MAX_WAIT_TIME;

    }

  }

  Order ret;

  ret = unassignedOrders->remove(best_index);

  assignedOrders.push_back(ret);

  return ret;

}

void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)

{

  if(msg->data.compare(0, 6, "FAILED") == 0)

  {

    int order_id = atoi(msg->data.substr(7).c_str());

    for(unsigned int i=0; i<assignedOrders.size(); i++)

    {

      if(assignedOrders[i].getid() == order_id)

      {

        task_failed(assignedOrders[i]);

        break;

      }

    }

  }

  else if(msg->data.compare(0, 7, "SUCCESS") == 0)

  {

    int order_id = atoi(msg->data.substr(8).c_str());

    for(unsigned int i=0; i<assignedOrders.size(); i++)

    {

      if(assignedOrders[i].getid() == order_id)

      {

        task_complete(assignedOrders[i]);

        break;

      }

    }

  }

  else if(msg->data.compare(0, 8, "GET_TASK") == 0)

  {

    ROS_INFO("SCHEDULER: got get task message");

    int robot = atoi(msg->data.substr(9).c_str());

    get_task(robot);

  }

  else if(msg->data.compare(0, 8, "REGISTER") == 0)

  {

    string robot_name = msg->data.substr(9);

    for(unsigned int i=0; i<robots.size(); i++)

    {

      if(robots[i].name.compare(robot_name) == 0)

      { 

        return;

      }

    }

    int id = robots.size() +1;

    Robot new_robot;

    new_robot.name = robot_name;

    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);

    new_robot.sched_status = NEW_ROBOT;

    robots.push_back(new_robot);

    std_msgs::String msg;

    std::stringstream ss;

    ss<<"REG_SUCCESS "<<id;

    msg.data = ss.str();

    new_robot.topic.publish(msg);

    spinOnce();

    ROS_INFO("Registration a success");

    assert(robots.size() > 0);

  }

  else

  {

    ROS_INFO("I got a bad message: %s", msg->data.c_str());

  }

  return;

}

/** @Brief: Continuously checks for waiting robots. If no robots are waiting,

    this function calls the waiting_dance() function. */

void Scheduler::run()

{

  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);

  ROS_INFO("I am a scheduler!. Now waiting for robots");

  while(robots.size() < 1 && ok())

  {

    spinOnce();

  }

  ROS_INFO("SCHEDULER: main loop");

	while (ok())

	{

    ROS_INFO("Scheduler running");

    spinOnce();

		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())

    {

      waiting_dance();

      spinOnce();

    }

    ROS_INFO("SCHEDULER: Setting task");

    Order next = get_next_item();

    Robot r = waitingRobots.front();

    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());

    std_msgs::String msg;

    std::stringstream ss;

    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();

    std::cout<<"msg: "<<ss.str()<<endl;

    msg.data = ss.str();

    r.topic.publish(msg);

    spinOnce();

    waitingRobots.front().sched_status = ORDERED_ROBOT;

		waitingRobots.pop();

	}

}

#ifndef _SCHEDULER_

#define _SCHEDULER_

#include "../helper_classes/PQWrapper.h"

#include "../helper_classes/Order.h"

#include "../Behavior.h"

#define NUM_TASKS 5

#define WAITING_ROBOT 1

#define NEW_ROBOT 2

#define ORDERED_ROBOT 3

typedef struct{

  std::string name;

  ros::Publisher topic;

  int sched_status;

} Robot;

class Scheduler : Behavior {

  std::vector<Robot> robots;

	std::queue<Robot> waitingRobots;

  PQWrapper* unassignedOrders;

	std::vector<Order> assignedOrders;

	void create_orders();

	void waiting_dance();

  void msg_callback(const std_msgs::String::ConstPtr& msg);

public:

  Scheduler(std::string scoutname, Sensors* sensors);

	~Scheduler();

	void get_task(int robot);

	void task_complete(Order o);

	void task_failed(Order o);

	Order get_next_item();

	void run();

  ros::Subscriber robot_to_sched;

};

#endif

#include "WH_Robot.h"

#include "../helper_classes/Order.h"

/** @Brief: warehouse robot constructor **/

WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):

            line_follow(scoutname, sensors) 

{

  nav_map = new navigationMap(scoutname, sensors);

  curr_task = DEFAULT_TASK;

  name = scoutname;

  reg_failed = 1;

  srand(0xDEADBEEF);

}

WH_Robot::~WH_Robot()

{

  delete(nav_map);

}

Duration WH_Robot::get_worst_case_time(State start_state, State target_state)

{

  return nav_map->get_worst_case_time(start_state, target_state);

}

/** @brief Drives from state 2 to the robot home base and goes to sleep for

 *  x seconds

 */

void WH_Robot::go_home(int x)

{

  unsigned int curr_state = nav_map->get_state();

  if(curr_state != 2)

  {

    ROS_WARN("Tried to go home from incorrect state %d", curr_state);

    return;

  }

  turn_straight();

  follow_line();

  turn_left();

  follow_line();

  Duration sleep_time(x);

  sleep_time.sleep();

  return;

}

void WH_Robot::leave_home()

{

  unsigned int curr_state = nav_map->get_state();

  if(curr_state != 2)

  {

    ROS_WARN("Tried to go home from incorrect state %d", curr_state);

    return;

  }

  turn_straight();

  follow_line();

  nav_map->update_state(ISTRAIGHT);

  turn_left();

  follow_line();

}

/** @brief Based on the given path, make

 *         the series of turns to follow this path,

 *         updating the current state in the navigation map as we do so   

 *  @param the Path to follow

 */

void WH_Robot::follow_path(Path path_to_follow){

  for(int i=0; i<path_to_follow.len; i++)

  {

    Turn t = path_to_follow.path[i];

    unsigned int curr_state = nav_map->get_state();

    nav_map->update_state(t);

    switch(t)

    {

      case ISTRAIGHT:

        turn_straight();

        follow_line();

        if(curr_state == 2)

        {

          turn_straight();

          follow_line();

          turn_straight();

          follow_line();

        }

        break;

      case ILEFT:

        turn_left();

        follow_line();

        break;

      case IRIGHT:

        if(9 <= curr_state && curr_state <= 12)

        {

          turn_straight();

          follow_line();

        }

        turn_right();

        follow_line();

        if(curr_state <= 4)

        {

          turn_straight();

          follow_line();

        }

        break;

      case IUTURN:

        u_turn();

        follow_line();

        break;

      case ISPOTTURN:

        spot_turn();

        follow_line();

        break;

    }

  }  

}

int WH_Robot::exec_task()

{

  assert(curr_task != DEFAULT_TASK);

  // remember where you started doing this task so we know where to return

  // State home_state = nav_map->get_state();

  // For now, use state 2.

  Path new_path = nav_map->shortest_path(curr_task->get_dest());

  curr_task->set_path(new_path);

  follow_path(new_path);

  //TODO: forklift yaaaay

  int did_task_complete;

  int error = rand() % 12;

  if(error < 9) //Fail with 1/4 probability

  {

    ROS_INFO("WH_robot: TASK COMPLETE!");

    did_task_complete = TASK_COMPLETED;

  }

  else

  {

    ROS_INFO("WH_robot: TASK FAILED!");

    did_task_complete = TASK_FAILED;

  }

  // For now use state 2 as home state

  // Path return_path = nav_map->shortest_path(home_state);

  Path return_path = nav_map->shortest_path(2);

  curr_task->set_path(return_path);

  follow_path(return_path);

  return did_task_complete;

}

void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)

{

  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)

  {

    id = atoi(msg->data.substr(12).c_str());

    reg_failed = 0;

  }

  else if(msg->data.compare(0, 8, "SET_TASK") == 0)

  {

    char* string = (char*)msg->data.c_str();

    char* data;

    data = strtok(string, " ");

    data = strtok(NULL, " ");

    int order_id = atoi(data);

    data = strtok(NULL, " ");

    Address source = atoi(data);

    data = strtok(NULL, " ");

    Address dest = atoi(data);

    Time start_time = ros::Time::now();

    Path path;

    path.len = 0;

    path.path = NULL;

    Duration est_time(0);

    curr_task = new Order(order_id, source, dest, start_time, path, est_time);

  }

  else

  {

    ROS_INFO("I got a bad message: %s", msg->data.c_str());

  }

}

void WH_Robot::run ()

{

  ROS_INFO("A WH_Robot has been created");

  robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);

  sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);

  ROS_INFO("I am a robot. Registering with scheduler...");

  while(reg_failed && ok())

  {

    std_msgs::String msg;

    std::stringstream ss;

    ss << "REGISTER "<<name;

    msg.data = ss.str();

    robot_to_sched.publish(msg);

    spinOnce();

  }

  follow_line();

  while(ok())

  {

    ROS_INFO("WH_ROBOT: Running main loop");

      std_msgs::String msg;

      std::stringstream ss;

      ss << "GET_TASK "<<id;

      msg.data = ss.str();

      robot_to_sched.publish(msg);

    while(curr_task == DEFAULT_TASK && ok()){

      spinOnce();

    }

    int error = exec_task();

    /** Go to robot home base */

    go_home(2);

    leave_home();

    if(error == TASK_COMPLETED)

    {

      std_msgs::String msg;

      std::stringstream ss;

      ss << "SUCCESS "<<curr_task->getid();

      msg.data = ss.str();

      robot_to_sched.publish(msg);

    }

    else //error == TASK_FAILED

    {

      std_msgs::String msg;

      std::stringstream ss;

      ss << "FAILED "<<curr_task->getid();

      msg.data = ss.str();

      robot_to_sched.publish(msg);

    }

    delete curr_task;

    curr_task = DEFAULT_TASK;

  }

}

void WH_Robot::set_task(Order order)

{

  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 

      order.get_start_time(), order.get_path(), order.get_est_time()); 

  return;

}

#ifndef _WH_ROBOT_

#define _WH_ROBOT_

#define DEFAULT_TASK NULL

#define TASK_COMPLETED 0

#define TASK_FAILED -1

#include "line_follow.h"

#include "../Behavior.h"

#include "navigationMap.h"

#include "../helper_classes/Order.h"

#include <assert.h>

#include <stdlib.h>

class WH_Robot : line_follow{

        std::string name;

        int id;

        int reg_failed;

        Order* curr_task;

        navigationMap* nav_map;

        Duration get_worst_case_time(State start_state, State target_state);

        int exec_task();

        void robot_callback(const std_msgs::String::ConstPtr& msg);

        void go_home(int x);

        void leave_home();

    public:

        WH_Robot(std::string scoutname, Sensors* sensors);

        ~WH_Robot();

        void run();

        void set_task(Order order);

        void follow_path(Path path_to_follow);

        ros::Publisher robot_to_sched;

        ros::Subscriber sched_to_robot;

};

#endif

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#include "maze_solve.h"

using namespace std;

// want to have a minimal working thing, use a big enough 

// static array and start in the middle

// we assume we are facing right, that affects where we store

// wall information

// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical

#define WALL -1

#define UNSEEN 0

#define SEEN 1

#define CRITICAL 2

// facings

#define UP 0

#define RIGHT 1

#define DOWN 2

#define LEFT 3

Duration sonar_update_time(1.5);

void maze_solve::run(){

    // TODO:first initialize map to all 0's

    ROS_INFO("Starting to solve the maze");

    // Go up to the first line.

    follow_line();

    // Turn the sonar on.

    sonar->set_on();

    sonar->set_range(0, 23);

    // Wait for the sonar to initialize.

    while(!look_around(25, 25, RIGHT) && ok())

    {

      spinOnce();      

    }

    // Solve the maze

    bool finished = solve(25,25, RIGHT);

    // Check and report final condition.

    if (finished)

        ROS_INFO("YAY! I have solved the maze");

    else

        ROS_INFO("NO! The maze is unsolvable");

}

bool maze_solve::solve(int row, int col, int dir)

{

    int initial_dir = dir;

    ROS_INFO("I am at direction %d", dir);

    // use backtracking to solve the maze

    if (at_destination())

        return true;

    // Wait for sonar to update.

    sonar_update_time.sleep();

    // this function should fill the adjacent cells around me with

    // wall's or paths

    while(!look_around(row, col, dir) && ok())

    {

        spinOnce();

    }

    /* try go up */

    if (map[row-1][col] != WALL && initial_dir != UP)

    {

    ROS_INFO("GOING UP!");

        // Turn up.

        turn_from_to(dir, UP);

        follow_line();

        // Solve recursively.

        bool solved = solve(row-1, col, DOWN);

        if (solved)

        {

            return solved;

        }

        else

        {

            //Update where we are.

            dir = UP;

        }

    }

    /* try right */

    if (map[row][col+1] != WALL && initial_dir != RIGHT)

    {

    ROS_INFO("GOING RIGHT!");

        // Turn right.

        turn_from_to(dir, RIGHT);

        follow_line();

        // Solve recursively.

        bool solved = solve(row, col+1, LEFT);

        if (solved)

        {

            return solved;

        }

        else

        {

            //Update where we are.

            dir = RIGHT;

        }

    }

    /* try down */

    if (map[row+1][col] != WALL && initial_dir != DOWN)

    {

    ROS_INFO("GOING DOWN!");

        // Turn down.

        turn_from_to(dir, DOWN);

        follow_line();

        // Solve recursively.

        bool solved = solve(row+1, col, UP);

        if (solved)

        {

            return solved;

        }

        else

        {

            //Update where we are.

            dir = DOWN;

        }

    }

    /* try left */

    if (map[row][col-1] != WALL && initial_dir != LEFT)

    {

    ROS_INFO("GOING LEFT!");

        // Turn down.

        turn_from_to(dir, LEFT);

        follow_line();

        // Solve recursively.

        bool solved = solve(row, col-1, RIGHT);

        if (solved)

        {

            return solved;

        }

        else

        {

            //Update where we are.

            dir = LEFT;

        }

    }

    ROS_INFO("DEAD END FOUND, TURNING BACK.");

    // we have exhausted all the options. This path is clearly a

    // dead end. go back to where we come from and return false.

    turn_from_to(dir, initial_dir);

    follow_line();

    return false;

}

// this function takes in the current direction and turns the scout

// into it intended direction

void maze_solve::turn_from_to(int current_dir, int intended_dir) {

    switch ((4 + intended_dir - current_dir) % 4) 

    {

        case 0:

            spot_turn();

            break;

        case 1:

            turn_left();

            break;

        case 2:

            turn_straight();

            break;

        case 3:

            turn_right();

            break;

    }

}

bool maze_solve::look_around(int row, int col, int dir)

{

    // look around current place using sonar

    // store whether or not

    // there is a wall into the map

    // stores at row col 2 if point is critical, 1 otherwise

    int* readings = sonar->get_sonar_readings();

    spinOnce();

/*

    // Look to the left.

    int left_distance = (readings[1] + readings[0] + readings[47])/3;

    // Look to the front.

    int front_distance = (readings[35] + readings[36] + readings[37])/3;

    // Look to the right.

    int right_distance = (readings[23] + readings[24] + readings[25])/3;

*/

    // Look to the left.

    int left_distance = readings[0];

    // Look to the front.

    int front_distance = readings[36];

    // Look to the right.

    int right_distance = readings[24];

    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);

    if (right_distance == 0 || front_distance == 0 || left_distance == 0)

      return false;

    switch (dir)

    {

        case UP:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row][col+1] = (left_distance < 500)?WALL:SEEN;

            map[row+1][col] = (front_distance < 500)?WALL:SEEN;

            map[row][col-1] = (right_distance < 500)?WALL:SEEN;

            break;

        case RIGHT:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row+1][col] = (left_distance < 500)?WALL:SEEN;

            map[row][col-1] = (front_distance < 500)?WALL:SEEN;

            map[row-1][col] = (right_distance < 500)?WALL:SEEN;

            break;

        case DOWN:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row][col-1] = (left_distance < 500)?WALL:SEEN;

            map[row-1][col] = (front_distance < 500)?WALL:SEEN;

            map[row][col+1] = (right_distance < 500)?WALL:SEEN;

            break;

        case LEFT:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row-1][col] = (left_distance < 500)?WALL:SEEN;

            map[row][col+1] = (front_distance < 500)?WALL:SEEN;

            map[row+1][col] = (right_distance < 500)?WALL:SEEN;

            break;

    }

    return true;

}

bool maze_solve::at_destination() 

{

    vector<uint32_t> readings = linesensor->query();

    if ( readings[0] > 200 &&

         readings[1] < 55 &&

         readings[2] < 55 &&

         readings[3] > 200 &&

         readings[4] > 200 &&

         readings[5] < 55 &&

         readings[6] < 55 &&

         readings[7] > 200 )

    {

        return true;

    }

    return false;

}

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#ifndef _MAZE_SOLVE_H_

#define _MAZE_SOLVE_H_

#include "line_follow.h"

class maze_solve: public line_follow

{

    public:

        maze_solve(std::string scoutname, Sensors* sensors):

            line_follow(scoutname, "maze_solve", sensors) {};

        void run();

    private:

        bool solve(int row, int col, int dir);

        void turn_from_to(int current_dir, int intended_dir);

        bool look_around(int row, int col, int dir);

        bool at_destination();

        int map[60][60];

};

#endif

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#include "trafficNavigation.h"

#include "../linefollowing/lineDrive.h"

#define DEBUG_INTERSECTION

#ifdef MAIN_NEW

#ifdef DEBUG_INTERSECTION

  #define DBG_USBS(str) usb_puts(str)

  #define DBG_USBI(x) usb_puti(x)

#else

  #define DBG_USBS(str)

  #define DBG_USBI(x)

#endif

static int state, sign, turnDir;

static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;

unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;

bool done;

//TODO: classes for lineDrive::doDrive and other functions in line_drive.h, clock functions, sending/receiving packets, encoder data, initializations, get ids, etc.

void trafficNavigation::run()

{

	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */

	id = get_robotid();

	sign = 0;

	delay_ms(500);

	//Test code

	state = SROAD;

	sendBuffer[1] = id;

	while (ok()) {

		/*DTN Finite State Machine*/

		switch(state){

		case SROAD:/*Following a normal road*/

			start();

			done = false;

			//finishes when reads a barcode: TODO: how do we detect intersections now?

			while(!done){

				sign = lineDrive::doDrive(180);

				switch(sign){

					case NORMAL:

					case FINISHED:

					case LOST:

					case ERROR:

						break;

					default:

						//we have a barcode!

						state = SINTERSECTION_ENTER;

						DBG_USBS("Read Barcode #:");

						DBG_USBI(sign);

						DBG_USBS("\n");

						done = true;

						break;

				}

			}

			break;

		case SINTERSECTION_ENTER:/*Entering, and in intersection*/

			stop();

			lineDrive::doDrive(0);

			DBG_USBS("STATE: SINTERSECTION_ENTER\n");

			/*Intersection queue:

			 *Each robot when entering the intersection will check for other robots

			 *in the intersection, and insert itself in a queue to go through.

			 */

			queuePrevBot = -1; //the bot that will drive before this bot

			queueNextBot = -1; //the bot that will drive after this bot

			resolvPrevBotID = -3; //in the case of a race, the bot that is

			                      //to enter the queue before this bot

			resolvPrevBotDraw = 0; //the random priority number that bot has

			resolvDraw = 0; //my random priority number

			intersectionNum = getIntersectNum(sign);

			if(intersectionNum == (char) -1){ //invalid

				state = SROAD;

				DBG_USBS("Barcode has invalid intersectionNum\n");

				break;

			}

			turnDir = validateTurn(sign, getTurnType(sign));

			if(turnDir == (char) -1){ //invalid

				state = SROAD;

				DBG_USBS("Barcode has invalid turn\n");

				break;

			}

			trafficNavigation::enterIntersection(); //sends wireless packet for entry

			state = SINTERSECTION_WAIT;

			rtc_reset(); //reset rtc for timeout wait for reply

			done = false;

			char retried = 0;

			while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);

				if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...

					trafficNavigation::enterIntersection();

					retried = 1;

				}

				switch (ret) {

					case KPLACEDINQUEUE:

						done = true;

						break;

					case KFAILEDTOQUEUE:

						DBG_USBS("Failed to queue\n");

						trafficNavigation::enterIntersection();

						rtc_reset();

						break;

					case KRESOLVINGENTER:

						state = SINTERSECTION_ENTER_RESOLV;

						done = true;

						break;

				}

			}

			break;

		case SINTERSECTION_ENTER_RESOLV:

			DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n");

			rtc_reset();

			done = false;

			retried = 0;

			while(rtc_get() < 4 && !done){

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);

				switch (ret) {

					case KRESOLVINGENTER:

						break;

					case KPLACEDINQUEUE:

						done = true;

						break;

					case KFAILEDTOQUEUE:

						DBG_USBS("Failed to queue\n");

						trafficNavigation::enterIntersection();

						rtc_reset();

						break;

				}

				//if resolvPrevBotID == -1, this indicates that

				//there was a prevbot before, but it has entered

				//the queue so it's our turn.

				if(!done && resolvPrevBotID == (char) -1 && !retried){

					trafficNavigation::enterIntersection();

					rtc_reset();

					retried = 1;

				//if resolvPrevBotID == -2, we have been

				//resolving but never have seen a bot with lower

				//priority than us. after the 6/16ths sec

				//timeout, assume we are first.

				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){

					//send a intersection reply to myself to

					//trigger other bots to enter queue.

					sendBuffer[0] = WINTERSECTIONREPLY;

					sendBuffer[2] = intersectionNum;

					sendBuffer[3] = id;

					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

					done = true;

					break;

				}

			}

			state = SINTERSECTION_WAIT;

			break;

		case SINTERSECTION_WAIT:/*Waiting in intersection */

			DBG_USBS("STATE: SINTERSECTION_WAIT\n");

			done = false;

			while(queuePrevBot != (char) -1){

				done = true;

				int ret = wirelessPacketHandle(state);

				if (ret==KFIRSTINQUEUE) state = SINTERSECTION_DRIVE;

			}

			//hack to make sure bot that just left intersection is

			//really out of the intersection.

			rtc_reset();

			while(rtc_get() < 2 && done){//wait one second

				wirelessPacketHandle(state);

			}

			state = SINTERSECTION_DRIVE;

			break;

		case SINTERSECTION_DRIVE:

			DBG_USBS("STATE: SINTERSECTION_DRIVE\n");

			start();

			turn(getIntersectType(sign), turnDir);

			//Exits intersection

			sendBuffer[0] = WINTERSECTIONEXIT;

			sendBuffer[2] = intersectionNum;//Intersection #

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			state = SROAD;

			break;

		default:

			DBG_USBS("I got stuck in an unknown state (Likely highway) My state is ");

			DBG_USBI(state);

		}

	}

}

int trafficNavigation::wirelessPacketHandle(int state){

	int dataLength = 0;

	unsigned char *packet = NULL;

	packet = wl_basic_do_default(&dataLength);

	/* sanity check */

	if(dataLength == 0 || packet == NULL) //no packet

		return ENOPACKET;

	if(dataLength != PACKET_LENGTH)

		return EPACKETLEN;

	DBG_USBS("Recieved Wireless Packet: ");

	for(int i = 0; i < dataLength; i++){

		DBG_USBI(packet[i]);

		DBG_USBS(" ");

	}

	DBG_USBS("\n");

	switch (packet[0]){

		case WROADENTRY: //[type, bot, road]

			return ENOACTION;

			break;

		case WROADEXIT: //[type, bot, road]

			return ENOACTION;

			break;

		case WROADSTOP: //[type, bot, road]

			return ENOACTION;

			break;

		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

						trafficNavigation::sendResolv(false);

						resolvPrevBotID = -2;

						return KRESOLVINGENTER;

						break;

					case SINTERSECTION_ENTER_RESOLV:

						return ENOACTION;

					case SINTERSECTION_WAIT:

					case SINTERSECTION_DRIVE:

						if(queueNextBot == (char) -1){

							sendBuffer[0] = WINTERSECTIONREPLY;

							sendBuffer[2] = intersectionNum;

							sendBuffer[3] = packet[1];

							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

							queueNextBot = packet[1];

							return KREPLIEDTOENTER;

						}

						break;

				}

			}

			break;

		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

					case SINTERSECTION_ENTER_RESOLV:

						if(packet[3] == id){ //Reply for me

							queuePrevBot = packet[1];

							return KPLACEDINQUEUE;

						} else {

							if(packet[3] == resolvPrevBotID)

								resolvPrevBotID = -1;

							return KFAILEDTOQUEUE;

						}

						break;

					default:

						return ENOACTION;

				}

			}

			break;

		case WINTERSECTIONEXIT: //[type, bot, intersection]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_WAIT:

						if(packet[1]==queuePrevBot){

							queuePrevBot=-1;

							return KFIRSTINQUEUE;

						}

				}

			}

			break;

		case WINTERSECTIONGO: //[type, bot, intersection]

			break;

		case WINTERSECTIONPOLICEENTRY: //[?]

			return ENOACTION;

			break;

		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]

			//in the case robots draw the same number these will be

			//arbitrated using the sending robot's id, prefering

			//lower ids

			DBG_USBS("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");

			DBG_USBI((int) resolvPrevBotID);

			DBG_USBS("\n");

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

					case SINTERSECTION_ENTER_RESOLV:

						if(resolvPrevBotID == (char) -3){

							DBG_USBS("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");

							trafficNavigation::sendResolv(false);

							resolvPrevBotID = -2;