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