Statistics
| Branch: | Revision:

scoutos / scout_avr / src / main.cpp @ 53349043

History | View | Annotate | Download (3.87 KB)

1
#include "ros.h"
2
#include "bom.h"
3
#include "range.h"
4
#include "stepper.h"
5
#include "orb.h"
6
#include "cliffSensor.h"
7
#include <messages/bom.h>
8
#include <messages/sonar_distance.h>
9
#include <messages/sonar_toggle.h>
10
#include <messages/sonar_set_scan.h>
11
#include <messages/set_headlights.h>
12
#include <messages/cliff_status_changed.h>
13
#include <util/delay.h>
14

    
15
/* Period of main loop in ms */
16
#define MAINLOOP_PERIOD 100//50
17

    
18
char range_enabled = 0;
19

    
20
void orb_callback(const messages::set_headlights& msg)
21
{
22
  orb_set0(msg.left_red, msg.left_green, msg.left_blue);
23
  orb_set1(msg.right_red, msg.right_green, msg.right_blue);
24
}
25

    
26
/* dammit, Priya, this capitalization just looks ridiculous */
27
void range_toggle_cb(const messages::sonar_toggleRequest& req,
28
    messages::sonar_toggleResponse& resp)
29
{
30
  range_enabled = req.set_on;
31
  if (range_enabled)
32
    step_enable();
33
  else
34
    step_disable();
35
  resp.ack = true;
36
}
37

    
38
void range_set_scan_cb(const messages::sonar_set_scanRequest& req,
39
    messages::sonar_set_scanResponse& resp)
40
{
41
  step_sweep_bounds(req.stop_l, req.stop_r);
42
  resp.ack = true;
43
}
44

    
45
int main()
46
{
47
  unsigned long now, next;
48
  unsigned int ranges[2];
49
  char i, id, cliff_status;
50

    
51
  ros::NodeHandle nh;
52
  nh.initNode();
53

    
54
  /* Stepper */
55
  // TODO ROS messages to set bounds
56
  step_init();
57
  step_dir(1);
58
  step_set_size(STEP_HALF);
59
  step_sweep_bounds(-26, 26);
60

    
61
  /* Range */
62
  range_init();
63
  messages::sonar_distance range_msg;
64
  ros::Publisher range_pub("sonar_distance", &range_msg);
65
  ros::ServiceServer
66
    <messages::sonar_toggleRequest, messages::sonar_toggleResponse>
67
    range_toggle("sonar_toggle", range_toggle_cb);
68
  ros::ServiceServer
69
    <messages::sonar_set_scanRequest, messages::sonar_set_scanResponse>
70
    range_set_scan("sonar_set_scan", range_set_scan_cb);
71
  nh.advertise(range_pub);
72
  nh.advertiseService(range_toggle);
73
  nh.advertiseService(range_set_scan);
74

    
75
  /* BOM */
76
  bom_init();
77
  messages::bom bom_msg;
78
  ros::Publisher bom_pub("bom", &bom_msg);
79
  nh.advertise(bom_pub);
80

    
81
  /* Headlights (aka Orbs) */
82
  //orb_init(); TODO debug orbs
83
  ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
84
      orb_callback);
85
  nh.subscribe(orb_sub);
86

    
87
  /* Cliff sensors */
88
  cliffSensor_init();
89
  messages::cliff_status_changed cliff_msg;
90
  ros::Publisher cliff_pub("cliff", &cliff_msg);
91
  nh.advertise(cliff_pub);
92

    
93
  id = 0;
94
  next = 0;
95
  while (1)
96
  {
97
    nh.spinOnce();
98

    
99
    /* Skip loop if the loop period hasn't passed yet */
100
    /* TODO if we need more exact timing, we can enter a tight loop when now
101
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
102
    now = nh.getHardware()->time();
103
    if (now < next) {
104
      continue;
105
    }
106
    next = now + MAINLOOP_PERIOD;
107

    
108
    /* Temporary, for BOM testing */
109
    id++;
110
    if (id == 0x40) {
111
      id = 0;
112
    }
113
    set_robot_id(id);
114
    bom_send(0);
115

    
116
    /* BOM */
117
    for (i = 0; i < 4; i++) {
118
      int msg = bom_get(i);
119
      if (msg != BOM_NO_DATA) {
120
        bom_msg.sender = bom_msg_get_robot_id(msg);
121
        bom_msg.send_dir = bom_msg_get_dir(msg);
122
        bom_msg.recv_dir = i;
123
        bom_pub.publish(&bom_msg);
124
      }
125
    }
126

    
127
    /* Stepper / range sensor */
128
    if (range_enabled) {
129
      step_sweep();
130
      range_measure(ranges);
131
      range_msg.stamp = nh.now();
132
      range_msg.pos = step_get_pos();
133
      range_msg.distance0 = ranges[0];
134
      range_msg.distance1 = ranges[1];
135
      range_msg.stamp = nh.now();
136
      range_pub.publish(&range_msg);
137
    }
138

    
139
    cliff_status = 0;
140
    if (read_cliffSensor_front())
141
      cliff_status |= messages::cliff_status_changed::CLIFF_FRONT;
142
    if (read_cliffSensor_left())
143
      cliff_status |= messages::cliff_status_changed::CLIFF_LEFT;
144
    if (read_cliffSensor_right())
145
      cliff_status |= messages::cliff_status_changed::CLIFF_RIGHT;
146
    if (cliff_status != cliff_msg.cliff_status) {
147
      cliff_msg.cliff_status = cliff_status;
148
      cliff_pub.publish(&cliff_msg);
149
    }
150

    
151
  }
152

    
153
  return 0;
154
}