Project

General

Profile

Revision 6c9146d5

ID6c9146d55fc6357be88eaa2185dd43e018001e75

Added by Thomas Mullins over 11 years ago

Began filling in ROSsy main function

View differences:

scout_avr/src/main.cpp
3 3
#include "ros.h"
4 4
#include "bom.h"
5 5
#include "range.h"
6
#include <std_msgs/Int16.h>
6
#include "stepper.h"
7
#include <std_msgs/Int16.h> // TODO remove
8
#include <bom/bom.h>
9
#include <sonar/sonar_distance.h>
10
#include <sonar/sonar_toggle.h>
11
#include <sonar/sonar_set_scan.h>
7 12
#include <util/delay.h>
8 13

  
9
ros::Publisher *pbom_pub;
14
/* Period of main loop in ms */
15
#define MAINLOOP_PERIOD 50
16

  
17
char range_enabled = 0;
10 18

  
11 19
void debug(const char *str)
12 20
{
......
16 24
{
17 25
}
18 26

  
27
/* dammit, Priya, this capitalization just looks ridiculous */
28
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
29
    sonar::sonar_toggleResponse& resp)
30
{
31
  range_enabled = req.set_on;
32
  resp.ack = true;
33
}
34

  
35
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
36
    sonar::sonar_set_scanResponse& resp)
37
{
38
  step_sweep_bounds(req.stop_l, req.stop_r);
39
  resp.ack = true;
40
}
41

  
19 42
int main()
20 43
{
44
  unsigned long now, next;
45
  unsigned int ranges[2];
21 46
  char i, id;
47

  
22 48
  ros::NodeHandle nh;
23
  bom::bom bom_msg;
24
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
25
  ros::Publisher bom_pub("bom", &bom_msg);
49
  nh.initNode();
26 50

  
27
  pbom_pub = &bom_pub;
51
  /* To be removed later; just an example of a subscirber */
52
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
53
  nh.subscribe(test_in);
28 54

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

  
66
  /* BOM */
31 67
  bom_init();
32
  nh.subscribe(test_in);
68
  bom::bom bom_msg;
69
  ros::Publisher bom_pub("bom", &bom_msg);
33 70
  nh.advertise(bom_pub);
34 71

  
35 72
  id = 0;
73
  next = 0;
36 74
  while (1)
37 75
  {
76
    nh.spinOnce();
77

  
78
    /* Skip loop if the loop period hasn't passed yet */
79
    /* TODO if we need more exact timing, we can enter a tight loop when now
80
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
81
    now = nh.getHardware()->time();
82
    if (now < next) {
83
      continue;
84
    }
85
    next = now + MAINLOOP_PERIOD;
86

  
87
    /* Temporary, for BOM testing */
38 88
    id++;
39 89
    if (id == 0x40) {
40 90
      id = 0;
41 91
    }
42 92
    set_robot_id(id);
43

  
44 93
    bom_send(id & 1);
94

  
95
    /* BOM */
45 96
    for (i = 0; i < 4; i++) {
46 97
      int msg = bom_get(i);
47 98
      if (msg != BOM_NO_DATA) {
......
52 103
      }
53 104
    }
54 105

  
55
    nh.spinOnce();
56
    _delay_ms(500);
106
    /* Stepper / range sensor */
107
    // TODO sweep stepper
108
    range_measure(ranges);
109
    range_msg.header.stamp = nh.now();
110
    range_msg.header.seq++;
111
    range_msg.pos = 0; // TODO fill this
112
    range_msg.distance0 = ranges[0];
113
    range_msg.distance1 = ranges[1];
114

  
57 115
  }
58 116

  
59 117
  return 0;
......
72 130
#include "Atmega128rfa1.h"
73 131
#include "range.h"
74 132
#include "bom.h"
133
#include "stepper.h"
75 134

  
76 135
Atmega128rfa1 avr;
77 136

  
......
89 148
  avr.init();
90 149
  range_init();
91 150
  bom_init();
151
  func step_sweep_cb = step_init(10);
152
  step_sweep_bounds(-26, 26);
153
  step_dir(1);
154
  step_sweep_speed(50);
92 155
  avr.puts("Hello!\r\n");
93 156
  while (1)
94 157
  {
95
    now = avr.time();
158
    step_sweep_cb();
159
    _delay_ms(10);
160
    /*now = avr.time();
96 161
    if (now > next) {
97
      next = now + 500;
162
      next = now + 50;*/
163
      //step_sweep_cb();
98 164
      /*ultoa(now, buf, 10);
99 165
      avr.puts(buf);
100 166
      avr.puts("\r\n");*/
101
      id++;
167
      /*id++;
102 168
      if (id == 0x40) {
103 169
        id = 0;
104 170
      }
105
      set_robot_id(id);
171
      set_robot_id(id);*/
106 172
      /*utoa(range_get(0), buf, 10);
107 173
      avr.puts("Range: ");
108 174
      avr.puts(buf);
109 175
      avr.puts(", ");
110 176
      utoa(range_get(1), buf, 10);
111 177
      avr.puts(buf);*/
112
      for (i = 0; i < 4; i++) {
178
      /*for (i = 0; i < 4; i++) {
113 179
        bom_send(i);
114 180
        int msg = bom_get(i);
115 181
        if (msg != BOM_NO_DATA) {
......
124 190
          avr.puts(buf);
125 191
          avr.puts(")\r\n");
126 192
        }
127
      }
128
    }
193
      }*/
194
    //}
129 195
  }
130 196
  return 0;
131 197
}

Also available in: Unified diff