Revision 2853d46b

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
}
scout_avr/src/stepper.cpp
12 12
 */
13 13

  
14 14
struct step_t {
15
  unsigned int speed; // time between steps in ms for sweep. Mininum 40ms
16
  unsigned int sweep_ms; //time between calls to sweep  
17
  unsigned int time;
18 15
  int pos; // position in rotation.
19 16
  int dir; // direction. -1 CCW. 1 CW. 0 OFF
17
  int step_size; // amount to add to position each step
20 18
  int ccw;
21 19
  int cw;
22
} volatile step;
20
} step;
23 21

  
24 22

  
25
func step_init(unsigned int call_ms)
23
void step_init()
26 24
{
27
  /* set update time for sweep */
28
  step.sweep_ms = call_ms;
29
  
30
  /* init pos, time, and dir to 0 */
25
  /* init pos and dir to 0 */
31 26
  step.pos = 0;
32
  step.time = 0;
33 27
  step.dir = 0;
34 28

  
35 29
  //set control pins as output
......
37 31
  DDRB |= ((1<<S_MS));
38 32

  
39 33
  //initiate to full steps
40
  PORTB &= (~(1<<S_MS));
34
  step_set_size(STEP_WHOLE);
41 35
 
42 36
  //initiate the step pin to be low. stepper steps on low to high
43 37
  PORTD &= (~(1<<S_STEP));
38
}
44 39

  
45
  //return function pointer to sweep to be called ever sweep_us uS. 
46
  return &(step_sweep);
40
void step_set_size(char size)
41
{
42
  if (size == STEP_WHOLE)
43
  {
44
    PORTB &= ~_BV(S_MS);
45
    step.step_size = 2;
46
  }
47
  else
48
  {
49
    PORTB |= _BV(S_MS);
50
    step.step_size = 1;
51
  }
47 52
}
48 53

  
49 54
/* set direction pin */
......
53 58
  switch(dir)
54 59
  {
55 60
    case 1:
56
         PORTD |= (1<<S_DIR);
61
      PORTD |= (1<<S_DIR);
57 62
      break;
58 63
    case -1:
59
         PORTD &= (~(1<<S_DIR));
64
      PORTD &= (~(1<<S_DIR));
60 65
      break;
61 66
  }
62 67
}
63 68

  
64
void step_halfstep()
65
{
66
  if(step.dir==0) return; //do not step if not enabled
67
  PORTB |= (1<<S_MS); //enable microstepping
68
  __asm__ __volatile__(
69
    "nop\n\t"
70
    "nop\n\t"
71
    "nop\n\t"
72
    "nop\n\t"
73
  ::);//wait 250 ns for microstepping to enable
74
  PORTD |= (1<<S_STEP); //step once
75
  _delay_us(1); //conform with step timing
76
  PORTD &= (~(1<<S_STEP)); //bring the step bin back down
77
  PORTB &= (~(1<<S_MS)); //disable microstepping
78
  if(step.dir==1) step.pos++; //keep track of pos. 1/2 step so add/sub 1
79
  else step.pos--;
80
}
81

  
82
void step_fullstep()
69
void step_do_step()
83 70
{
84 71
  if(step.dir==0) return; //do not step if not enabled
85 72
  PORTD |= (1<<S_STEP); //step once 
86 73
  _delay_us(1); //conform with step timing
87 74
  PORTD &= (~(1<<S_STEP)); //bring the step bin back down
88
  if(step.dir==1) step.pos+=2; // full step so add/sub 2
89
  else step.pos-=2;
75
  if(step.dir==1) step.pos += step.step_size;
76
  else step.pos -= step.step_size;
90 77
}
91 78

  
92 79
void step_flush()
......
94 81
  PORTD &= (~(1<<S_STEP)); //bring the step bin back down
95 82
}
96 83

  
97
/* tick every speed ms in sweep mode */
98
void step_sweep_speed(unsigned int speed)
99
{
100
  if(speed<40) step.speed = 40;
101
  else step.speed = speed;
102
}
103

  
104

  
105 84
//ccw must be less than 0 and cw must be greater than 0
106 85
void step_sweep_bounds(int ccw, int cw)
107 86
{
......
111 90

  
112 91
void step_sweep()
113 92
{
114
  step.time += step.sweep_ms;
115
  if(step.time >= step.speed)
116
  {
117
    step_fullstep();
118
    if((step.dir == 1) && (step.cw <= step.pos)) step_dir(-1);
119
    else if((step.dir == -1) && (step.ccw >= step.pos)) step_dir(1);
120
    step.time = 0;
121
  }
93
  step_do_step();
94
  if((step.dir == 1) && (step.cw <= step.pos)) step_dir(-1);
95
  else if((step.dir == -1) && (step.ccw >= step.pos)) step_dir(1);
122 96
}
scout_avr/src/stepper.h
6 6

  
7 7
#define S_MS      PB7
8 8

  
9
//define the type func as a pointer to a void function with no parameters
10
typedef void (*func)();
9
#define STEP_WHOLE 2
10
#define STEP_HALF 1
11 11

  
12
/*returns a funciton pointer that must be called at regular intervals
13
* as specified by the call_us argument
14
*/
15
func step_init(unsigned int call_us);
12
/* Initializes the stepper with whole steps and no direction. step_sweep_bounds
13
 * and step_dir should be called before using step_sweep. */
14
void step_init();
16 15

  
17 16

  
17
void step_set_size(char size);
18 18
void step_dir(int dir);
19
void step_halfstep();
20
void step_fullstep();
19
void step_do_step();
21 20
void step_flush();
22
void step_sweep_speed(unsigned int speed);
23 21
void step_sweep_bounds(int ccw, int cw);
24 22
void step_sweep();
25 23

  

Also available in: Unified diff