root / trunk / code / projects / diagnostic_station / station / test_encoders.c @ 1297
History | View | Annotate | Download (2.99 KB)
1 |
#include "test_encoders.h" |
---|---|
2 |
#include "global.h" |
3 |
#include "comm_robot.h" |
4 |
|
5 |
#include "hardware.h" |
6 |
|
7 |
// ******************
|
8 |
// ** Data sending **
|
9 |
// ******************
|
10 |
|
11 |
static void send_encoder_data (uint8_t num, uint8_t direction, |
12 |
int16_t *data_station, int16_t *data_robot, uint8_t num_measurements) |
13 |
{ |
14 |
if (direction!=motor_direction_off)
|
15 |
{ |
16 |
usb_puts ("data encoder ");
|
17 |
usb_puti (num); |
18 |
usb_puts (" ");
|
19 |
usb_puts (motor_direction_string (direction)); |
20 |
usb_puts (" ");
|
21 |
|
22 |
for (uint8_t i=0; i<num_measurements; ++i) |
23 |
{ |
24 |
usb_putc (' ');
|
25 |
usb_puti (data_station[i]); |
26 |
usb_putc ('/');
|
27 |
usb_puti (data_robot[i]); |
28 |
} |
29 |
|
30 |
usb_puts (NL); |
31 |
} |
32 |
} |
33 |
|
34 |
|
35 |
// ************************
|
36 |
// ** Internal functions **
|
37 |
// ************************
|
38 |
|
39 |
static void test_encoders_direction (uint8_t direction1, uint8_t direction2, uint8_t num_measurements, uint8_t velocity, uint16_t on_delay, uint16_t off_delay) |
40 |
{ |
41 |
// Allocate space for the data on the stack
|
42 |
int16_t *data_station_l =malloc (num_measurements*sizeof (uint16_t));
|
43 |
int16_t *data_station_r =malloc (num_measurements*sizeof (uint16_t));
|
44 |
int16_t *data_robot_l =malloc (num_measurements*sizeof (uint16_t));
|
45 |
int16_t *data_robot_r =malloc (num_measurements*sizeof (uint16_t));
|
46 |
|
47 |
usb_puts ("# Resetting encoders" NL);
|
48 |
dynamos_reset (); |
49 |
robot_reset_encoders (); |
50 |
|
51 |
for (uint8_t m=0; m<num_measurements; ++m) |
52 |
{ |
53 |
robot_set_motors (direction1, velocity, direction2, velocity); |
54 |
delay_ms (on_delay); |
55 |
robot_set_motors_off (); |
56 |
delay_ms (off_delay); |
57 |
|
58 |
// Read the station dynamos
|
59 |
dynamos_read (&(data_station_l[m]), &(data_station_r[m])); |
60 |
|
61 |
// Read the robot encoders
|
62 |
robot_read_encoders (&(data_robot_l[m]), &(data_robot_r[m])); |
63 |
} |
64 |
|
65 |
send_encoder_data (0, direction1, data_station_l, data_robot_l, num_measurements);
|
66 |
send_encoder_data (1, direction2, data_station_r, data_robot_r, num_measurements);
|
67 |
|
68 |
free (data_robot_r); |
69 |
free (data_robot_l); |
70 |
free (data_station_r); |
71 |
free (data_station_l); |
72 |
} |
73 |
|
74 |
|
75 |
// **********************
|
76 |
// ** Public functions **
|
77 |
// **********************
|
78 |
|
79 |
#define NUM 4 |
80 |
#define VEL 200 |
81 |
#define ON_DELAY 500 |
82 |
#define OFF_DELAY 800 |
83 |
|
84 |
void test_encoder_all (void) |
85 |
{ |
86 |
usb_puts("# Testing encoders" NL);
|
87 |
|
88 |
test_encoders_direction (motor_direction_forward , motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY); |
89 |
test_encoders_direction (motor_direction_backward, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY); |
90 |
|
91 |
usb_puts("# Testing encoders finished" NL);
|
92 |
} |
93 |
|
94 |
void test_encoder (uint8_t num)
|
95 |
{ |
96 |
if (num==1) |
97 |
{ |
98 |
test_encoders_direction (motor_direction_forward , motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY); |
99 |
test_encoders_direction (motor_direction_backward, motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY); |
100 |
} |
101 |
else if (num==2) |
102 |
{ |
103 |
test_encoders_direction (motor_direction_off, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY); |
104 |
test_encoders_direction (motor_direction_off, motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY); |
105 |
} |
106 |
} |