Revision 86b48573 scout_avr/src/main.cpp

View differences:

scout_avr/src/main.cpp
12 12
#include <util/delay.h>
13 13

  
14 14
/* Period of main loop in ms */
15
#define MAINLOOP_PERIOD 50
15
#define MAINLOOP_PERIOD 1000//50
16 16

  
17 17
char range_enabled = 0;
18 18

  
......
55 55
  /* Range */
56 56
  range_init();
57 57
  sonar::sonar_distance range_msg;
58
  ros::Publisher range_distance("sonar_distance", &range_msg);
58
  ros::Publisher range_pub("sonar_distance", &range_msg);
59 59
  ros::ServiceServer
60 60
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
61 61
    range_toggle("sonar_toggle", range_toggle_cb);
62 62
  ros::ServiceServer
63 63
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
64 64
    range_set_scan("sonar_set_scan", range_set_scan_cb);
65
  nh.advertise(range_pub);
65 66

  
66 67
  /* BOM */
67 68
  bom_init();
......
111 112
    range_msg.pos = 0; // TODO fill this
112 113
    range_msg.distance0 = ranges[0];
113 114
    range_msg.distance1 = ranges[1];
115
    range_pub.publish(&range_msg);
114 116

  
115 117
  }
116 118

  
......
142 144
int main()
143 145
{
144 146
  char buf[20];
145
  int i;
146
  char id = 0;
147
  //int i;
148
  //char id = 0;
147 149
  unsigned long now, next = 0;
150
  unsigned int ranges[2];
148 151
  avr.init();
149 152
  range_init();
150 153
  bom_init();
151
  func step_sweep_cb = step_init(10);
154
  /*func step_sweep_cb = step_init(10);
152 155
  step_sweep_bounds(-26, 26);
153 156
  step_dir(1);
154
  step_sweep_speed(50);
157
  step_sweep_speed(50);*/
155 158
  avr.puts("Hello!\r\n");
156 159
  while (1)
157 160
  {
158
    step_sweep_cb();
159
    _delay_ms(10);
160
    /*now = avr.time();
161
    /*step_sweep_cb();
162
    _delay_ms(10);*/
163
    now = avr.time();
161 164
    if (now > next) {
162
      next = now + 50;*/
165
      next = now + 500;
163 166
      //step_sweep_cb();
164 167
      /*ultoa(now, buf, 10);
165 168
      avr.puts(buf);
......
169 172
        id = 0;
170 173
      }
171 174
      set_robot_id(id);*/
172
      /*utoa(range_get(0), buf, 10);
175
      range_measure(ranges);
176
      utoa(ranges[0], buf, 10);
173 177
      avr.puts("Range: ");
174 178
      avr.puts(buf);
175 179
      avr.puts(", ");
176
      utoa(range_get(1), buf, 10);
177
      avr.puts(buf);*/
180
      utoa(ranges[1], buf, 10);
181
      avr.puts(buf);
182
      avr.puts("\r\n");
178 183
      /*for (i = 0; i < 4; i++) {
179 184
        bom_send(i);
180 185
        int msg = bom_get(i);
......
191 196
          avr.puts(")\r\n");
192 197
        }
193 198
      }*/
194
    //}
199
    }
195 200
  }
196 201
  return 0;
197 202
}

Also available in: Unified diff