Revision 2853d46b 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 1000//50
15
#define MAINLOOP_PERIOD 100//50
16 16

  
17 17
char range_enabled = 0;
18 18

  
......
52 52
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
53 53
  nh.subscribe(test_in);
54 54

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

  
55 62
  /* Range */
56 63
  range_init();
57 64
  sonar::sonar_distance range_msg;
......
105 112
    }
106 113

  
107 114
    /* Stepper / range sensor */
108
    // TODO sweep stepper
109
    range_measure(ranges);
110
    range_msg.header.stamp = nh.now();
111
    range_msg.header.seq++;
112
    range_msg.pos = 0; // TODO fill this
113
    range_msg.distance0 = ranges[0];
114
    range_msg.distance1 = ranges[1];
115
    range_pub.publish(&range_msg);
115
    if (range_enabled) {
116
      step_sweep();
117
      range_measure(ranges);
118
      range_msg.header.stamp = nh.now();
119
      range_msg.header.seq++;
120
      range_msg.pos = 0; // TODO fill this
121
      range_msg.distance0 = ranges[0];
122
      range_msg.distance1 = ranges[1];
123
      range_pub.publish(&range_msg);
124
    }
116 125

  
117 126
  }
118 127

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

Also available in: Unified diff