Revision 6350051e

View differences:

scout/libscout/CMakeLists.txt
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32 32
set(MAIN_FILES src/Sensors.cpp src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
33
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)
33
FILE(GLOB BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/behaviors/*.cpp")
34
FILE(GLOB TEST_BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/test_behaviors/*.cpp")
34 35

  
35
set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp src/behaviors/maze_solve.cpp)
36 36
set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp)
37 37
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)
38 38

  
scout/libscout/generate_behavior_lists.py
1
#!/usr/bin/python
2

  
3
# Usage:
4
#
5
# python generate_behavior_lists.py
6
#
7
# Parses to use only the final folder and file name for each source file.
8

  
9
import sys
10
import os
11

  
12
AUTOGEN_HEADER = '''/**
13
 ******************************************************************************
14
 * WARNING: THIS IS AN AUTOGENERATED FILE!
15
 *
16
 * Editing this file is USELESS. It will be overwritten during the build.
17
 * To properly edit this file, edit its corresponding *.template.* file, which
18
 * a script run by CMake uses to generate this file.
19
 *
20
 * This file was generated via rules in src/generate_behavior_lists.py.
21
 *
22
 * Please see that file for a description of syntax and procedure for
23
 * creating auto-generated files.
24
 *
25
 * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
26
 * THANK YOU.
27
 *****************************************************************************
28
 */
29

  
30
'''
31

  
32
# These are the codes this script will replace in template files.
33
FULL_PATH_CODE = 'AUTOGEN_BEHAVIOR_FULL_PATH'
34
LAST_PATH_CODE = 'AUTOGEN_BEHAVIOR_LAST_PATH'
35
BASE_NAME_CODE = 'AUTOGEN_BEHAVIOR_BASE_NAME'
36

  
37
# Different ways to refer to files in behaviors/ and test_behaviors/.
38
full_paths = []
39
last_paths = []
40
base_names = []
41

  
42
# Mapping of generated files to their templates
43
R_FILES = {'src/BehaviorList.cpp' : 'src/BehaviorList.template.cpp',
44
           'src/BehaviorList.h'   : 'src/BehaviorList.template.h'}
45

  
46
# Parse the src/behaviors and src/test_behaviors folders
47
cur_dir = os.getcwd()
48
for path, dirname, fnames in os.walk('src'):
49
    if path == 'src/behaviors' or path == 'src/test_behaviors':
50
        path_parts = path.split('/')
51

  
52
        for f in sorted(fnames):
53
            # The pause_scout behavior needs to go first!
54
            if f.endswith('.h') and 'pause_scout' in f:
55
                full_paths.insert(0, os.path.join(cur_dir, path, f))
56
                last_paths.insert(0, os.path.join(path_parts[-1], f))
57
                base_names.insert(0, f.split('.')[0])
58
            # Everything else goes in alphabetical order
59
            elif f.endswith('.h'):
60
                full_paths.append(os.path.join(cur_dir, path, f))
61
                last_paths.append(os.path.join(path_parts[-1], f))
62
                base_names.append(f.split('.')[0])
63

  
64
# Now, a dictionary of codes to replace -> things to replace them with.
65
replacements = {FULL_PATH_CODE : full_paths,
66
                LAST_PATH_CODE : last_paths,
67
                BASE_NAME_CODE : base_names}
68

  
69
# Write all the replacements in R_FILES
70
for outfile_name in R_FILES.keys():
71
    with open(outfile_name, 'w') as outfile:
72
        with open(R_FILES[outfile_name], 'r') as infile:
73
            
74
            # Write an obvious header so no one will edit our nicely
75
            # generated file
76
            outfile.write(AUTOGEN_HEADER)
77

  
78
            for line in infile.readlines():
79
                # If the line needs replacing, don't write the orgininal
80
                # to the output file; rather, write many versions of the
81
                # line replaced with each replacement value.
82
                replaced = False
83
                for r_key in replacements:
84
                    if r_key in line:
85
                        replaced = True
86
                        for b in replacements[r_key]:
87
                            outfile.write(line.replace(r_key, b))
88
                # If it doesn't need replacing, just write the original
89
                if not replaced:
90
                    outfile.write(line)
scout/libscout/src/BehaviorList.cpp
1
#include "BehaviorList.h"
1
/**
2
 ******************************************************************************
3
 * WARNING: THIS IS AN AUTOGENERATED FILE!
4
 *
5
 * Editing this file is USELESS. It will be overwritten during the build.
6
 * To properly edit this file, edit its corresponding *.template.* file, which
7
 * a script run by CMake uses to generate this file.
8
 *
9
 * This file was generated via rules in src/generate_behavior_lists.py.
10
 *
11
 * Please see that file for a description of syntax and procedure for
12
 * creating auto-generated files.
13
 *
14
 * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
15
 * THANK YOU.
16
 *****************************************************************************
17
 */
2 18

  
19
#include "BehaviorList.h"
3 20

  
4 21
BehaviorList::BehaviorList()
5 22
{
6 23
  behavior_list.push_back(behavior<pause_scout>);
7
  behavior_list.push_back(behavior<draw_cw_circle>);
8
  behavior_list.push_back(behavior<draw_ccw_circle>);
9 24
  behavior_list.push_back(behavior<Odometry>);
25
  behavior_list.push_back(behavior<draw_ccw_circle>);
26
  behavior_list.push_back(behavior<draw_cw_circle>);
27
  behavior_list.push_back(behavior<line_follow>);
10 28
  behavior_list.push_back(behavior<navigationMap>);
29
  behavior_list.push_back(behavior<wl_test>);
11 30
  behavior_list.push_back(behavior<Scheduler>);
12 31
  behavior_list.push_back(behavior<WH_Robot>);
13
  behavior_list.push_back(behavior<line_follow>);
14
  behavior_list.push_back(behavior<wl_test>);
15 32
  behavior_list.push_back(behavior<maze_solve>);
16 33
  return;
17 34
}
scout/libscout/src/BehaviorList.h
1
/**
2
 ******************************************************************************
3
 * WARNING: THIS IS AN AUTOGENERATED FILE!
4
 *
5
 * Editing this file is USELESS. It will be overwritten during the build.
6
 * To properly edit this file, edit its corresponding *.template.* file, which
7
 * a script run by CMake uses to generate this file.
8
 *
9
 * This file was generated via rules in src/generate_behavior_lists.py.
10
 *
11
 * Please see that file for a description of syntax and procedure for
12
 * creating auto-generated files.
13
 *
14
 * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
15
 * THANK YOU.
16
 *****************************************************************************
17
 */
18

  
1 19
#ifndef _BEHAVIOR_LIST_H_
2 20
#define _BEHAVIOR_LIST_H_
3 21

  
4 22
#include "Behavior.h"
5
#include "behaviors/line_follow.h"
6
#include "behaviors/draw_cw_circle.h"
7
#include "behaviors/draw_ccw_circle.h"
23
#include "Sensors.h"
24
#include "behaviors/pause_scout.h"
8 25
#include "behaviors/Odometry.h"
26
#include "behaviors/draw_ccw_circle.h"
27
#include "behaviors/draw_cw_circle.h"
28
#include "behaviors/line_follow.h"
9 29
#include "behaviors/navigationMap.h"
10
#include "behaviors/Scheduler.h"
11
#include "behaviors/WH_Robot.h"
12 30
#include "behaviors/wl_test.h"
13
#include "behaviors/pause_scout.h"
14
#include "behaviors/maze_solve.h"
15
#include "Sensors.h"
31
#include "test_behaviors/Scheduler.h"
32
#include "test_behaviors/WH_Robot.h"
33
#include "test_behaviors/maze_solve.h"
16 34

  
17 35
template<typename T> Behavior* behavior(std::string scoutname, Sensors* sensors){ return (Behavior*)new T(scoutname, sensors); } 
18 36

  
scout/libscout/src/behaviors/Scheduler.cpp
1
#include "Scheduler.h"
2
#include "../helper_classes/Order.h"
3

  
4
using namespace std;
5

  
6
/** @Brief: Initialize data structures for the scheduler. */
7
Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
8
            Behavior(scoutname, "Scheduler", sensors)
9
{
10
  unassignedOrders = new PQWrapper(NUM_TASKS);
11

  
12
	create_orders();
13
}
14

  
15
/** @Brief: Free allocatetd memory. */
16
Scheduler::~Scheduler()
17
{
18
  delete unassignedOrders;
19
}
20

  
21
/** @Brief: Add robot to the waiting queue. 
22
 *  A robot calls this function with itself
23
 *  and gets pushed on a list of waiting robots.
24
 *  When a task is availaible the scheduler, removes
25
 *  the robot of the waiting queue, and gives it a
26
 *  task.
27
 */
28
void Scheduler::get_task(int robot)
29
{
30
  Robot my_robot = robots[robot-1];
31
  if(my_robot.sched_status != WAITING_ROBOT)
32
  {
33
	  waitingRobots.push(my_robot);
34
    my_robot.sched_status = WAITING_ROBOT;
35
  }
36
}
37

  
38
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
39
void Scheduler::create_orders()
40
{
41
  Path p;
42
  p.len = 0;
43
  p.path = NULL;
44
  Time t = ros::Time::now();
45
  Duration end(0);
46
  Duration five(5);
47
  Duration three(3);
48
  Duration ten(10);
49
	Order a(1,0,13,t+ten,p,end);
50
  Order b(2,0,14,t+five,p,end);
51
  Order c(3,0,15,t+three,p,end);
52
  Order d(4,0,16,t,p,end);
53
	
54
	unassignedOrders->insert(a);
55
	unassignedOrders->insert(b);
56
	unassignedOrders->insert(c);
57
	unassignedOrders->insert(d);
58
}
59

  
60
/** @Brief: This is a confirmation that the task is complete. 
61
    This function removes the order from assignedOrders. */
62
void Scheduler::task_complete(Order o)
63
{
64
  ROS_INFO("Scheduler: Task id %d was completed", o.getid());
65
  for (unsigned int i=0; i<assignedOrders.size(); i++)
66
  {
67
    if (assignedOrders[i].getid()==o.getid())
68
    {
69
      assignedOrders.erase(assignedOrders.begin()+i);
70
    }
71
  }
72
}
73

  
74
/** @Brief: This is a confirmation that the task failed. 
75
    This function places the order back on the PQWrapper. */
76
void Scheduler::task_failed(Order o)
77
{
78
  ROS_INFO("Scheduler: Task id %d was failed", o.getid());
79
	task_complete(o);
80
	unassignedOrders->insert(o);
81
}
82

  
83
/** @Brief: Do a waiting dance. */
84
void Scheduler::waiting_dance()
85
{ 
86
    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
87
}
88

  
89
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
90
Order Scheduler::get_next_item()
91
{
92
  Time t = ros::TIME_MAX; 
93
  double time = t.toSec();
94

  
95
  Order* best;
96
  int best_index;
97
  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
98
  {
99
    Order order = unassignedOrders->peek(i);
100

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

  
103
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
104
    {
105
      best = &order;
106
      best_index = i;
107
      time = end_time.toSec() + MAX_WAIT_TIME;
108
    }
109
  }
110

  
111
  Order ret;
112
  ret = unassignedOrders->remove(best_index);
113
  assignedOrders.push_back(ret);
114
  return ret;
115
}
116

  
117
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
118
{
119
  if(msg->data.compare(0, 6, "FAILED") == 0)
120
  {
121
    int order_id = atoi(msg->data.substr(7).c_str());
122
    for(unsigned int i=0; i<assignedOrders.size(); i++)
123
    {
124
      if(assignedOrders[i].getid() == order_id)
125
      {
126
        task_failed(assignedOrders[i]);
127
        break;
128
      }
129
    }
130
  }
131
  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
132
  {
133
    int order_id = atoi(msg->data.substr(8).c_str());
134
    for(unsigned int i=0; i<assignedOrders.size(); i++)
135
    {
136
      if(assignedOrders[i].getid() == order_id)
137
      {
138
        task_complete(assignedOrders[i]);
139
        break;
140
      }
141
    }
142
  }
143
  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
144
  {
145
    ROS_INFO("SCHEDULER: got get task message");
146
    int robot = atoi(msg->data.substr(9).c_str());
147
    get_task(robot);
148
  }
149
  else if(msg->data.compare(0, 8, "REGISTER") == 0)
150
  {
151
    string robot_name = msg->data.substr(9);
152
    for(unsigned int i=0; i<robots.size(); i++)
153
    {
154
      if(robots[i].name.compare(robot_name) == 0)
155
      { 
156
        return;
157
      }
158
    }
159

  
160
    int id = robots.size() +1;
161
    Robot new_robot;
162
    new_robot.name = robot_name;
163
    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
164
    new_robot.sched_status = NEW_ROBOT;
165
    robots.push_back(new_robot);
166

  
167
    std_msgs::String msg;
168
    std::stringstream ss;
169
    ss<<"REG_SUCCESS "<<id;
170
    msg.data = ss.str();
171
    new_robot.topic.publish(msg);
172
    spinOnce();
173

  
174
    ROS_INFO("Registration a success");
175

  
176
    assert(robots.size() > 0);
177
  }
178
  else
179
  {
180
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
181
  }
182

  
183
  return;
184
}
185

  
186
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
187
    this function calls the waiting_dance() function. */
188
void Scheduler::run()
189
{
190
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
191
  ROS_INFO("I am a scheduler!. Now waiting for robots");
192
  while(robots.size() < 1 && ok())
193
  {
194
    spinOnce();
195
  }
196
  ROS_INFO("SCHEDULER: main loop");
197
	while (ok())
198
	{
199
    ROS_INFO("Scheduler running");
200
    spinOnce();
201
		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
202
    {
203
      waiting_dance();
204
      spinOnce();
205
    }
206
  
207
    ROS_INFO("SCHEDULER: Setting task");
208
    Order next = get_next_item();
209
    Robot r = waitingRobots.front();
210

  
211
    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
212
    std_msgs::String msg;
213
    std::stringstream ss;
214
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
215
    std::cout<<"msg: "<<ss.str()<<endl;
216
    msg.data = ss.str();
217
    r.topic.publish(msg);
218
  
219
    spinOnce();
220

  
221
    waitingRobots.front().sched_status = ORDERED_ROBOT;
222
		waitingRobots.pop();
223
	}
224
}
225

  
226

  
227

  
228

  
scout/libscout/src/behaviors/Scheduler.h
1
#ifndef _SCHEDULER_
2
#define _SCHEDULER_
3

  
4
#include "../helper_classes/PQWrapper.h"
5
#include "../helper_classes/Order.h"
6
#include "../Behavior.h"
7

  
8
#define NUM_TASKS 5
9
#define WAITING_ROBOT 1
10
#define NEW_ROBOT 2
11
#define ORDERED_ROBOT 3
12

  
13
typedef struct{
14
  std::string name;
15
  ros::Publisher topic;
16
  int sched_status;
17
} Robot;
18

  
19
class Scheduler : Behavior {
20
  std::vector<Robot> robots;
21
	std::queue<Robot> waitingRobots;
22

  
23
  PQWrapper* unassignedOrders;
24
	std::vector<Order> assignedOrders;
25

  
26
	void create_orders();
27

  
28
	void waiting_dance();
29

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

  
32
public:
33
  Scheduler(std::string scoutname, Sensors* sensors);
34
	~Scheduler();
35
	
36
	void get_task(int robot);
37
	
38
	void task_complete(Order o);
39
	void task_failed(Order o);
40
	
41
	
42
	Order get_next_item();
43
	
44
	void run();
45

  
46
  ros::Subscriber robot_to_sched;
47
    
48
};
49
#endif
scout/libscout/src/behaviors/WH_Robot.cpp
1
#include "WH_Robot.h"
2
#include "../helper_classes/Order.h"
3

  
4
/** @Brief: warehouse robot constructor **/
5
WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):
6
            line_follow(scoutname, sensors) 
7
{
8
  nav_map = new navigationMap(scoutname, sensors);
9
  curr_task = DEFAULT_TASK;
10
  name = scoutname;
11
  reg_failed = 1;
12
  srand(0xDEADBEEF);
13
}
14

  
15
WH_Robot::~WH_Robot()
16
{
17
  delete(nav_map);
18
}
19

  
20
Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
21
{
22
  return nav_map->get_worst_case_time(start_state, target_state);
23
}
24

  
25
/** @brief Drives from state 2 to the robot home base and goes to sleep for
26
 *  x seconds
27
 */
28
void WH_Robot::go_home(int x)
29
{
30
  unsigned int curr_state = nav_map->get_state();
31
  if(curr_state != 2)
32
  {
33
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
34
    return;
35
  }
36
  turn_straight();
37
  follow_line();
38
  turn_left();
39
  follow_line();
40
  Duration sleep_time(x);
41
  sleep_time.sleep();
42
  return;
43
}
44

  
45
void WH_Robot::leave_home()
46
{
47
  unsigned int curr_state = nav_map->get_state();
48
  if(curr_state != 2)
49
  {
50
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
51
    return;
52
  }
53
  turn_straight();
54
  follow_line();
55
  nav_map->update_state(ISTRAIGHT);
56
  turn_left();
57
  follow_line();
58
}
59

  
60
/** @brief Based on the given path, make
61
 *         the series of turns to follow this path,
62
 *         updating the current state in the navigation map as we do so   
63
 *  @param the Path to follow
64
 */
65
void WH_Robot::follow_path(Path path_to_follow){
66
  for(int i=0; i<path_to_follow.len; i++)
67
  {
68
    Turn t = path_to_follow.path[i];
69
    unsigned int curr_state = nav_map->get_state();
70

  
71
    nav_map->update_state(t);
72
    switch(t)
73
    {
74
      case ISTRAIGHT:
75
        turn_straight();
76
        follow_line();
77
        if(curr_state == 2)
78
        {
79
          turn_straight();
80
          follow_line();
81
          turn_straight();
82
          follow_line();
83
        }
84
        break;
85
      case ILEFT:
86
        turn_left();
87
        follow_line();
88
        break;
89
      case IRIGHT:
90
        if(9 <= curr_state && curr_state <= 12)
91
        {
92
          turn_straight();
93
          follow_line();
94
        }
95
        turn_right();
96
        follow_line();
97
        if(curr_state <= 4)
98
        {
99
          turn_straight();
100
          follow_line();
101
        }
102
        break;
103
      case IUTURN:
104
        u_turn();
105
        follow_line();
106
        break;
107
      case ISPOTTURN:
108
        spot_turn();
109
        follow_line();
110
        break;
111
    }
112
  }  
113
}
114

  
115
int WH_Robot::exec_task()
116
{
117
  assert(curr_task != DEFAULT_TASK);
118
  
119
  // remember where you started doing this task so we know where to return
120
  // State home_state = nav_map->get_state();
121
  // For now, use state 2.
122
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
123
  curr_task->set_path(new_path);
124

  
125
  follow_path(new_path);
126
   
127
  //TODO: forklift yaaaay
128

  
129
  int did_task_complete;
130

  
131
  int error = rand() % 12;
132
  if(error < 9) //Fail with 1/4 probability
133
  {
134
    ROS_INFO("WH_robot: TASK COMPLETE!");
135
    did_task_complete = TASK_COMPLETED;
136
  }
137
  else
138
  {
139
    ROS_INFO("WH_robot: TASK FAILED!");
140
    did_task_complete = TASK_FAILED;
141
  }
142
  
143
  // For now use state 2 as home state
144
  // Path return_path = nav_map->shortest_path(home_state);
145
  Path return_path = nav_map->shortest_path(2);
146
  curr_task->set_path(return_path);
147
  
148
  follow_path(return_path);
149

  
150
  return did_task_complete;
151
}
152

  
153
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
154
{
155
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
156
  {
157
    id = atoi(msg->data.substr(12).c_str());
158
    reg_failed = 0;
159
  }
160
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
161
  {
162
    char* string = (char*)msg->data.c_str();
163
    char* data;
164
    data = strtok(string, " ");
165

  
166
    data = strtok(NULL, " ");
167
    int order_id = atoi(data);
168

  
169
    data = strtok(NULL, " ");
170
    Address source = atoi(data);
171

  
172
    data = strtok(NULL, " ");
173
    Address dest = atoi(data);
174

  
175
    Time start_time = ros::Time::now();
176
    Path path;
177
    path.len = 0;
178
    path.path = NULL;
179
    Duration est_time(0);
180

  
181
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
182
  }
183
  else
184
  {
185
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
186
  }
187
}
188

  
189
void WH_Robot::run ()
190
{
191
  ROS_INFO("A WH_Robot has been created");
192

  
193
  robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
194
  sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
195

  
196
  ROS_INFO("I am a robot. Registering with scheduler...");
197
  while(reg_failed && ok())
198
  {
199
    std_msgs::String msg;
200
    std::stringstream ss;
201
    ss << "REGISTER "<<name;
202
    msg.data = ss.str();
203
    robot_to_sched.publish(msg);
204

  
205
    spinOnce();
206
  }
207

  
208
  follow_line();
209

  
210
  while(ok())
211
  {
212
    ROS_INFO("WH_ROBOT: Running main loop");
213

  
214

  
215
      std_msgs::String msg;
216
      std::stringstream ss;
217
      ss << "GET_TASK "<<id;
218
      msg.data = ss.str();
219
      robot_to_sched.publish(msg);
220

  
221
    while(curr_task == DEFAULT_TASK && ok()){
222
      spinOnce();
223
    }
224

  
225
    int error = exec_task();
226

  
227
    /** Go to robot home base */
228
    go_home(2);
229
    leave_home();
230

  
231
    if(error == TASK_COMPLETED)
232
    {
233
      std_msgs::String msg;
234
      std::stringstream ss;
235
      ss << "SUCCESS "<<curr_task->getid();
236
      msg.data = ss.str();
237
      robot_to_sched.publish(msg);
238
    }
239
    else //error == TASK_FAILED
240
    {
241
      std_msgs::String msg;
242
      std::stringstream ss;
243
      ss << "FAILED "<<curr_task->getid();
244
      msg.data = ss.str();
245
      robot_to_sched.publish(msg);
246
    }
247
    delete curr_task;
248
    curr_task = DEFAULT_TASK;
249

  
250
  }
251
}
252

  
253
void WH_Robot::set_task(Order order)
254
{
255
  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
256
      order.get_start_time(), order.get_path(), order.get_est_time()); 
257
  return;
258
}
259

  
scout/libscout/src/behaviors/WH_Robot.h
1
#ifndef _WH_ROBOT_
2
#define _WH_ROBOT_
3

  
4
#define DEFAULT_TASK NULL
5
#define TASK_COMPLETED 0
6
#define TASK_FAILED -1
7

  
8
#include "line_follow.h"
9
#include "../Behavior.h"
10
#include "navigationMap.h"
11
#include "../helper_classes/Order.h"
12
#include <assert.h>
13
#include <stdlib.h>
14

  
15
class WH_Robot : line_follow{
16
        std::string name;
17
        int id;
18

  
19
        int reg_failed;
20

  
21
        Order* curr_task;
22
        navigationMap* nav_map;
23

  
24
        Duration get_worst_case_time(State start_state, State target_state);
25
        int exec_task();
26

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

  
29
        void go_home(int x);
30
        void leave_home();
31

  
32
    public:
33
        WH_Robot(std::string scoutname, Sensors* sensors);
34
        ~WH_Robot();
35
        void run();
36

  
37
        void set_task(Order order);
38
        void follow_path(Path path_to_follow);
39

  
40
        ros::Publisher robot_to_sched;
41
        ros::Subscriber sched_to_robot;
42
};
43

  
44
#endif
scout/libscout/src/behaviors/maze_solve.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
#include "maze_solve.h"
27

  
28
using namespace std;
29

  
30

  
31
// want to have a minimal working thing, use a big enough 
32
// static array and start in the middle
33
// we assume we are facing right, that affects where we store
34
// wall information
35
// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
36
#define WALL -1
37
#define UNSEEN 0
38
#define SEEN 1
39
#define CRITICAL 2
40
// facings
41
#define UP 0
42
#define RIGHT 1
43
#define DOWN 2
44
#define LEFT 3
45

  
46
Duration sonar_update_time(1.5);
47

  
48
void maze_solve::run(){
49
    
50
    // TODO:first initialize map to all 0's
51
    ROS_INFO("Starting to solve the maze");
52
    // Go up to the first line.
53
    follow_line();
54
    // Turn the sonar on.
55
    sonar->set_on();
56
    sonar->set_range(0, 23);
57

  
58
    // Wait for the sonar to initialize.
59
    while(!look_around(25, 25, RIGHT) && ok())
60
    {
61
      spinOnce();      
62
    }
63

  
64
    // Solve the maze
65
    bool finished = solve(25,25, RIGHT);
66

  
67
    // Check and report final condition.
68
    if (finished)
69
        ROS_INFO("YAY! I have solved the maze");
70
    else
71
        ROS_INFO("NO! The maze is unsolvable");
72
}
73

  
74
bool maze_solve::solve(int row, int col, int dir)
75
{
76
    int initial_dir = dir;
77

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

  
80
    // use backtracking to solve the maze
81
    if (at_destination())
82
        return true;
83

  
84
    // Wait for sonar to update.
85
    sonar_update_time.sleep();
86

  
87
    // this function should fill the adjacent cells around me with
88
    // wall's or paths
89
    while(!look_around(row, col, dir) && ok())
90
    {
91
        spinOnce();
92
    }
93

  
94
    /* try go up */
95
    if (map[row-1][col] != WALL && initial_dir != UP)
96
    {
97
    ROS_INFO("GOING UP!");
98
        // Turn up.
99
        turn_from_to(dir, UP);
100
        follow_line();
101
        // Solve recursively.
102
        bool solved = solve(row-1, col, DOWN);
103
        if (solved)
104
        {
105
            return solved;
106
        }
107
        else
108
        {
109
            //Update where we are.
110
            dir = UP;
111
        }
112
    }
113
    /* try right */
114
    if (map[row][col+1] != WALL && initial_dir != RIGHT)
115
    {
116
    ROS_INFO("GOING RIGHT!");
117
        // Turn right.
118
        turn_from_to(dir, RIGHT);
119
        follow_line();
120
        // Solve recursively.
121
        bool solved = solve(row, col+1, LEFT);
122
        if (solved)
123
        {
124
            return solved;
125
        }
126
        else
127
        {
128
            //Update where we are.
129
            dir = RIGHT;
130
        }
131
    }
132
    /* try down */
133
    if (map[row+1][col] != WALL && initial_dir != DOWN)
134
    {
135
    ROS_INFO("GOING DOWN!");
136
        // Turn down.
137
        turn_from_to(dir, DOWN);
138
        follow_line();
139
        // Solve recursively.
140
        bool solved = solve(row+1, col, UP);
141
        if (solved)
142
        {
143
            return solved;
144
        }
145
        else
146
        {
147
            //Update where we are.
148
            dir = DOWN;
149
        }
150
    }
151
    /* try left */
152
    if (map[row][col-1] != WALL && initial_dir != LEFT)
153
    {
154
    ROS_INFO("GOING LEFT!");
155
        // Turn down.
156
        turn_from_to(dir, LEFT);
157
        follow_line();
158
        // Solve recursively.
159
        bool solved = solve(row, col-1, RIGHT);
160
        if (solved)
161
        {
162
            return solved;
163
        }
164
        else
165
        {
166
            //Update where we are.
167
            dir = LEFT;
168
        }
169
    }
170

  
171
    ROS_INFO("DEAD END FOUND, TURNING BACK.");
172
    // we have exhausted all the options. This path is clearly a
173
    // dead end. go back to where we come from and return false.
174
    turn_from_to(dir, initial_dir);
175
    follow_line();
176
    return false;
177
}
178

  
179
// this function takes in the current direction and turns the scout
180
// into it intended direction
181
void maze_solve::turn_from_to(int current_dir, int intended_dir) {
182
    switch ((4 + intended_dir - current_dir) % 4) 
183
    {
184
        case 0:
185
            spot_turn();
186
            break;
187
        case 1:
188
            turn_left();
189
            break;
190
        case 2:
191
            turn_straight();
192
            break;
193
        case 3:
194
            turn_right();
195
            break;
196
    }
197
}
198

  
199
bool maze_solve::look_around(int row, int col, int dir)
200
{
201
    // look around current place using sonar
202
    // store whether or not
203
    // there is a wall into the map
204
    // stores at row col 2 if point is critical, 1 otherwise
205
    
206
    int* readings = sonar->get_sonar_readings();
207
    spinOnce();
208

  
209
/*
210
    // Look to the left.
211
    int left_distance = (readings[1] + readings[0] + readings[47])/3;
212
    // Look to the front.
213
    int front_distance = (readings[35] + readings[36] + readings[37])/3;
214
    // Look to the right.
215
    int right_distance = (readings[23] + readings[24] + readings[25])/3;
216
*/
217
    // Look to the left.
218
    int left_distance = readings[0];
219
    // Look to the front.
220
    int front_distance = readings[36];
221
    // Look to the right.
222
    int right_distance = readings[24];
223

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

  
226
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
227
      return false;
228

  
229
    switch (dir)
230
    {
231
        case UP:
232
            // If the distance is less than 500, mark the area as a wall otherwise
233
            // mark it as seen.
234
            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
235
            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
236
            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
237
            break;
238
        case RIGHT:
239
            // If the distance is less than 500, mark the area as a wall otherwise
240
            // mark it as seen.
241
            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
242
            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
243
            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
244
            break;
245
        case DOWN:
246
            // If the distance is less than 500, mark the area as a wall otherwise
247
            // mark it as seen.
248
            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
249
            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
250
            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
251
            break;
252
        case LEFT:
253
            // If the distance is less than 500, mark the area as a wall otherwise
254
            // mark it as seen.
255
            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
256
            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
257
            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
258
            break;
259
    }
260

  
261
    return true;
262
}
263

  
264
bool maze_solve::at_destination() 
265
{
266
    vector<uint32_t> readings = linesensor->query();
267
    if ( readings[0] > 200 &&
268
         readings[1] < 55 &&
269
         readings[2] < 55 &&
270
         readings[3] > 200 &&
271
         readings[4] > 200 &&
272
         readings[5] < 55 &&
273
         readings[6] < 55 &&
274
         readings[7] > 200 )
275
    {
276
        return true;
277
    }
278
    return false;
279
}
scout/libscout/src/behaviors/maze_solve.h
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
#ifndef _MAZE_SOLVE_H_
27
#define _MAZE_SOLVE_H_
28

  
29
#include "line_follow.h"
30

  
31
class maze_solve: public line_follow
32
{
33
    public:
34
        maze_solve(std::string scoutname, Sensors* sensors):
35
            line_follow(scoutname, "maze_solve", sensors) {};
36
        void run();
37
    private:
38
        bool solve(int row, int col, int dir);
39
        void turn_from_to(int current_dir, int intended_dir);
40
        bool look_around(int row, int col, int dir);
41
        bool at_destination();
42

  
43
        int map[60][60];
44
};
45
#endif
46

  
scout/libscout/src/behaviors/trafficNavigation.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
#include "trafficNavigation.h"
27
#include "../linefollowing/lineDrive.h"
28

  
29
#define DEBUG_INTERSECTION
30
#ifdef MAIN_NEW
31

  
32
#ifdef DEBUG_INTERSECTION
33
  #define DBG_USBS(str) usb_puts(str)
34
  #define DBG_USBI(x) usb_puti(x)
35
#else
36
  #define DBG_USBS(str)
37
  #define DBG_USBI(x)
38
#endif
39

  
40
static int state, sign, turnDir;
41
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;
42
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;
43
bool done;
44

  
45

  
46
//TODO: classes for lineDrive::doDrive and other functions in line_drive.h, clock functions, sending/receiving packets, encoder data, initializations, get ids, etc.
47
void trafficNavigation::run()
48
{
49

  
50
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
51
	
52
	id = get_robotid();
53
	sign = 0;
54
	delay_ms(500);
55
	
56
	//Test code
57
	state = SROAD;
58

  
59
	sendBuffer[1] = id;
60

  
61
	
62
	while (ok()) {
63
		/*DTN Finite State Machine*/
64
		switch(state){
65
		case SROAD:/*Following a normal road*/
66
			start();
67
			done = false;
68
			
69
			//finishes when reads a barcode: TODO: how do we detect intersections now?
70
			while(!done){
71
				sign = lineDrive::doDrive(180);
72
				switch(sign){
73
					case NORMAL:
74
					case FINISHED:
75
					case LOST:
76
					case ERROR:
77
						break;
78
					default:
79
						//we have a barcode!
80
						state = SINTERSECTION_ENTER;
81
						DBG_USBS("Read Barcode #:");
82
						DBG_USBI(sign);
83
						DBG_USBS("\n");
84
						done = true;
85
						break;
86

  
87
				}
88
			}
89
			break;
90
		case SINTERSECTION_ENTER:/*Entering, and in intersection*/
91
			stop();
92
			lineDrive::doDrive(0);
93

  
94
			DBG_USBS("STATE: SINTERSECTION_ENTER\n");
95

  
96
			/*Intersection queue:
97
			 *Each robot when entering the intersection will check for other robots
98
			 *in the intersection, and insert itself in a queue to go through.
99
			 */
100
			queuePrevBot = -1; //the bot that will drive before this bot
101
			queueNextBot = -1; //the bot that will drive after this bot
102
			resolvPrevBotID = -3; //in the case of a race, the bot that is
103
			                      //to enter the queue before this bot
104
			resolvPrevBotDraw = 0; //the random priority number that bot has
105
			resolvDraw = 0; //my random priority number
106
			
107
			intersectionNum = getIntersectNum(sign);
108
			if(intersectionNum == (char) -1){ //invalid
109
				state = SROAD;
110
				DBG_USBS("Barcode has invalid intersectionNum\n");
111
				break;
112
			}
113
			turnDir = validateTurn(sign, getTurnType(sign));
114
			if(turnDir == (char) -1){ //invalid
115
				state = SROAD;
116
				DBG_USBS("Barcode has invalid turn\n");
117
				break;
118
			}
119

  
120
			trafficNavigation::enterIntersection(); //sends wireless packet for entry
121
			state = SINTERSECTION_WAIT;
122

  
123
			rtc_reset(); //reset rtc for timeout wait for reply
124
			done = false;
125
			char retried = 0;
126
			while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue
127
				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);
128
				if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...
129
					trafficNavigation::enterIntersection();
130
					retried = 1;
131
				}
132
				switch (ret) {
133
					case KPLACEDINQUEUE:
134
						done = true;
135
						break;
136
					case KFAILEDTOQUEUE:
137
						DBG_USBS("Failed to queue\n");
138
						trafficNavigation::enterIntersection();
139
						rtc_reset();
140
						break;
141
					case KRESOLVINGENTER:
142
						state = SINTERSECTION_ENTER_RESOLV;
143
						done = true;
144
						break;
145
				}
146
			}
147
			break;
148

  
149
		case SINTERSECTION_ENTER_RESOLV:
150
			DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n");
151

  
152
			rtc_reset();
153
			done = false;
154
			retried = 0;
155
			while(rtc_get() < 4 && !done){
156
				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
157
				switch (ret) {
158
					case KRESOLVINGENTER:
159
						break;
160
					case KPLACEDINQUEUE:
161
						done = true;
162
						break;
163
					case KFAILEDTOQUEUE:
164
						DBG_USBS("Failed to queue\n");
165
						trafficNavigation::enterIntersection();
166
						rtc_reset();
167
						break;
168
				}
169
				//if resolvPrevBotID == -1, this indicates that
170
				//there was a prevbot before, but it has entered
171
				//the queue so it's our turn.
172
				if(!done && resolvPrevBotID == (char) -1 && !retried){
173
					trafficNavigation::enterIntersection();
174
					rtc_reset();
175
					retried = 1;
176
				//if resolvPrevBotID == -2, we have been
177
				//resolving but never have seen a bot with lower
178
				//priority than us. after the 6/16ths sec
179
				//timeout, assume we are first.
180
				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){
181
					//send a intersection reply to myself to
182
					//trigger other bots to enter queue.
183
					sendBuffer[0] = WINTERSECTIONREPLY;
184
					sendBuffer[2] = intersectionNum;
185
					sendBuffer[3] = id;
186
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
187

  
188
					done = true;
189
					break;
190
				}
191
			}
192
			state = SINTERSECTION_WAIT;
193
			break;
194
		case SINTERSECTION_WAIT:/*Waiting in intersection */
195

  
196

  
197
			DBG_USBS("STATE: SINTERSECTION_WAIT\n");
198

  
199
			done = false;
200
			while(queuePrevBot != (char) -1){
201
				done = true;
202
				int ret = wirelessPacketHandle(state);
203
				if (ret==KFIRSTINQUEUE) state = SINTERSECTION_DRIVE;
204
			}
205
			//hack to make sure bot that just left intersection is
206
			//really out of the intersection.
207
			rtc_reset();
208
			while(rtc_get() < 2 && done){//wait one second
209
				wirelessPacketHandle(state);
210
			}
211

  
212
			state = SINTERSECTION_DRIVE;
213
			break;
214
		case SINTERSECTION_DRIVE:
215
			DBG_USBS("STATE: SINTERSECTION_DRIVE\n");
216
			start();
217
			turn(getIntersectType(sign), turnDir);
218
			
219
			
220
			//Exits intersection
221
			sendBuffer[0] = WINTERSECTIONEXIT;
222
			sendBuffer[2] = intersectionNum;//Intersection #
223
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
224

  
225
			state = SROAD;
226
			break;
227
		default:
228
			DBG_USBS("I got stuck in an unknown state (Likely highway) My state is ");
229
			DBG_USBI(state);
230
		}
231
	}
232
}
233

  
234
int trafficNavigation::wirelessPacketHandle(int state){
235
	int dataLength = 0;
236
	unsigned char *packet = NULL;
237
	packet = wl_basic_do_default(&dataLength);
238
	
239
	/* sanity check */
240
	if(dataLength == 0 || packet == NULL) //no packet
241
		return ENOPACKET;
242

  
243
	if(dataLength != PACKET_LENGTH)
244
		return EPACKETLEN;
245

  
246
	DBG_USBS("Recieved Wireless Packet: ");
247
	for(int i = 0; i < dataLength; i++){
248
		DBG_USBI(packet[i]);
249
		DBG_USBS(" ");
250
	}
251
	DBG_USBS("\n");
252

  
253
	switch (packet[0]){
254
		case WROADENTRY: //[type, bot, road]
255
			return ENOACTION;
256
			break;
257
		case WROADEXIT: //[type, bot, road]
258
			return ENOACTION;
259
			break;
260
		case WROADSTOP: //[type, bot, road]
261
			return ENOACTION;
262
			break;
263
		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]
264
			if (packet[2] == intersectionNum){
265
				switch (state){
266
					case SINTERSECTION_ENTER:
267
						trafficNavigation::sendResolv(false);
268
						resolvPrevBotID = -2;
269
						return KRESOLVINGENTER;
270
						break;
271
					case SINTERSECTION_ENTER_RESOLV:
272
						return ENOACTION;
273
					case SINTERSECTION_WAIT:
274
					case SINTERSECTION_DRIVE:
275
						if(queueNextBot == (char) -1){
276
							sendBuffer[0] = WINTERSECTIONREPLY;
277
							sendBuffer[2] = intersectionNum;
278
							sendBuffer[3] = packet[1];
279
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
280
							queueNextBot = packet[1];
281
							return KREPLIEDTOENTER;
282
						}
283
						break;
284

  
285
				}
286
			}
287
			break;
288
		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]
289
			if (packet[2] == intersectionNum){
290
				switch (state){
291
					case SINTERSECTION_ENTER:
292
					case SINTERSECTION_ENTER_RESOLV:
293
						if(packet[3] == id){ //Reply for me
294
							queuePrevBot = packet[1];
295
							return KPLACEDINQUEUE;
296
						} else {
297
							if(packet[3] == resolvPrevBotID)
298
								resolvPrevBotID = -1;
299
							return KFAILEDTOQUEUE;
300
						}
301
						break;
302
					default:
303
						return ENOACTION;
304
				}
305
			}
306
			break;
307
		case WINTERSECTIONEXIT: //[type, bot, intersection]
308
			if (packet[2] == intersectionNum){
309
				switch (state){
310
					case SINTERSECTION_WAIT:
311
						if(packet[1]==queuePrevBot){
312
							queuePrevBot=-1;
313
							return KFIRSTINQUEUE;
314
						}
315
				}
316
			}
317
			break;
318
		case WINTERSECTIONGO: //[type, bot, intersection]
319
			break;
320
		case WINTERSECTIONPOLICEENTRY: //[?]
321
			return ENOACTION;
322
			break;
323
		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]
324
			//in the case robots draw the same number these will be
325
			//arbitrated using the sending robot's id, prefering
326
			//lower ids
327
			DBG_USBS("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");
328
			DBG_USBI((int) resolvPrevBotID);
329
			DBG_USBS("\n");
330
			if (packet[2] == intersectionNum){
331
				switch (state){
332
					case SINTERSECTION_ENTER:
333
					case SINTERSECTION_ENTER_RESOLV:
334
						if(resolvPrevBotID == (char) -3){
335
							DBG_USBS("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");
336
							trafficNavigation::sendResolv(false);
337
							resolvPrevBotID = -2;
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff