Revision 2853d46b
ID | 2853d46ba3da7879828050adece3cb330e999876 |
Changed stepper to always step in step_sweep
Also added whole/half step as a separate setting, with a single
step_do_step function. Also added stepper to main.cpp.
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