Revision 1214
Implemented motors test
trunk/code/projects/diagnostic_station/station/test_motors.c | ||
---|---|---|
2 | 2 |
#include "global.h" |
3 | 3 |
|
4 | 4 |
#include "comm_robot.h" |
5 |
#include "hardware_encoders.h" |
|
5 | 6 |
|
6 |
// NB don't produce overflows! |
|
7 |
#define vel_min 100 |
|
8 |
#define vel_max 250 |
|
9 |
#define vel_inc 10 |
|
10 |
#define vel_delay 200 |
|
11 |
|
|
12 |
static void test_motors_direction_velocity (uint8_t direction1, uint8_t direction2, uint8_t velocity) |
|
7 |
static void send_motor_data (uint8_t num, uint8_t direction, char *acceleration_string, |
|
8 |
uint8_t num_steps, uint8_t *data_pwm, int16_t *data_velocity) |
|
13 | 9 |
{ |
14 |
robot_set_motors (direction1, velocity, direction2, velocity); |
|
15 |
delay_ms (vel_delay); |
|
10 |
if (direction!=motor_direction_off) |
|
11 |
{ |
|
12 |
usb_puts ("data motor "); |
|
13 |
usb_puti (num); |
|
14 |
usb_puts (" "); |
|
15 |
usb_puts (motor_direction_string (direction)); |
|
16 |
usb_puts (" "); |
|
17 |
usb_puts (acceleration_string); |
|
18 |
usb_puts (" "); |
|
19 |
|
|
20 |
for (uint8_t i=0; i<num_steps; ++i) |
|
21 |
{ |
|
22 |
usb_putc (' '); |
|
23 |
usb_puti (data_pwm[i]); |
|
24 |
usb_putc ('/'); |
|
25 |
usb_puti (data_velocity[i]); |
|
26 |
} |
|
16 | 27 |
|
17 |
// wait (steady) |
|
18 |
// |
|
19 |
// reset_encoders (); |
|
20 |
// for (measurements) |
|
21 |
// { |
|
22 |
// measure left/right |
|
23 |
// wait |
|
24 |
// } |
|
28 |
usb_puts (NL); |
|
29 |
} |
|
25 | 30 |
} |
26 | 31 |
|
32 |
static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t pwm_start, uint8_t num_steps, int8_t pwm_step, char *acceleration_string) |
|
33 |
{ |
|
34 |
// Allocate space for the data on the stack |
|
35 |
uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t )); |
|
36 |
int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t)); |
|
37 |
int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t)); |
|
27 | 38 |
|
28 |
#define acceleration_increasing 1 |
|
29 |
#define acceleration_decreasing 2 |
|
39 |
uint8_t pwm=pwm_start; |
|
40 |
for (uint8_t i=0; i<num_steps; ++i) |
|
41 |
{ |
|
42 |
robot_set_motors (direction1, pwm, direction2, pwm); |
|
43 |
// TODO better |
|
44 |
#define vel_delay 400 |
|
45 |
delay_ms (vel_delay); // TODO wait steady |
|
30 | 46 |
|
31 |
// TODO: pass number of steps instead |
|
32 |
static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t vel_start, uint8_t vel_end, int8_t vel_step, char *acceleration_string) |
|
33 |
{ |
|
34 |
// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4 |
|
35 |
// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is |
|
36 |
// easy and performance is not an issue here anyway. |
|
47 |
data_pwm[i]=pwm; |
|
37 | 48 |
|
38 |
// Distinguish between start>=end/start<end because we have to use the corresponding stop condition. |
|
39 |
if (vel_end>=vel_start) |
|
40 |
for (uint16_t vel=vel_start; vel<=vel_end; vel+=vel_step) |
|
41 |
test_motors_direction_velocity (direction1, direction2, vel); |
|
42 |
else |
|
43 |
for (uint16_t vel=vel_start; vel>=vel_end; vel+=vel_step) |
|
44 |
test_motors_direction_velocity (direction1, direction2, vel); |
|
49 |
encoders_reset (); |
|
50 |
// TODO delay |
|
51 |
encoders_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
|
45 | 52 |
|
46 |
- // Send data |
|
47 |
for (uint8_t i=1; i<=2; ++i) |
|
48 |
{ |
|
49 |
uint8_t direction=(i==1)?direction1:direction2; |
|
50 |
|
|
51 |
if (direction!=motor_direction_off) |
|
52 |
{ |
|
53 |
usb_puts ("data motor "); |
|
54 |
usb_puti (i); |
|
55 |
usb_puts (" "); |
|
56 |
usb_puts (motor_direction_string (direction)); |
|
57 |
usb_puts (" "); |
|
58 |
usb_puts (acceleration_string); |
|
59 |
usb_puts (" "); |
|
60 |
|
|
61 |
usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL); |
|
62 |
} |
|
53 |
pwm+=pwm_step; |
|
63 | 54 |
} |
55 |
|
|
56 |
send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l); |
|
57 |
send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r); |
|
58 |
|
|
59 |
robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
|
60 |
|
|
61 |
free (data_velocity_r); |
|
62 |
free (data_velocity_l); |
|
63 |
free (data_pwm); |
|
64 | 64 |
} |
65 | 65 |
|
66 | 66 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
67 | 67 |
{ |
68 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_max, vel_inc, "increasing"); |
|
69 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_min, -vel_inc, "decreasing"); |
|
68 |
#define vel_inc 10 |
|
69 |
#define vel_steps 10 |
|
70 |
#define vel_min 140 |
|
71 |
#define vel_max 250 |
|
72 |
|
|
73 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing"); |
|
74 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing"); |
|
70 | 75 |
} |
71 | 76 |
|
72 | 77 |
|
trunk/code/projects/diagnostic_station/station/hardware_encoders.c | ||
---|---|---|
48 | 48 |
} |
49 | 49 |
|
50 | 50 |
|
51 |
void encoders_read (int16_t *values)
|
|
51 |
void encoders_read (int16_t *left, int16_t *right)
|
|
52 | 52 |
{ |
53 |
usb_puts ("# Reading encoder values"); |
|
54 | 53 |
SYNC |
55 | 54 |
{ |
56 |
*values = count_l;
|
|
57 |
*(values+1) = count_r;
|
|
55 |
*left= count_l;
|
|
56 |
*right = count_r;
|
|
58 | 57 |
} |
59 | 58 |
} |
60 | 59 |
|
trunk/code/projects/diagnostic_station/station/hardware_encoders.h | ||
---|---|---|
6 | 6 |
|
7 | 7 |
void encoders_init (void); |
8 | 8 |
|
9 |
void encoders_read (int16_t *values);
|
|
9 |
void encoders_read (int16_t *left, int16_t *right);
|
|
10 | 10 |
void encoders_reset (void); |
11 | 11 |
|
12 | 12 |
#endif |
Also available in: Unified diff