Revision 1215
Implemented encoders test
test_encoders.c | ||
---|---|---|
2 | 2 |
#include "global.h" |
3 | 3 |
#include "comm_robot.h" |
4 | 4 |
|
5 |
#include "hardware.h" |
|
6 |
|
|
7 |
// TODO as parameters |
|
5 | 8 |
#define num_measurements 4 |
6 | 9 |
#define velocity 200 |
7 | 10 |
#define on_delay 500 |
8 | 11 |
#define off_delay 800 |
9 | 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 |
|
|
10 | 45 |
static void test_encoders_direction (uint8_t direction1, uint8_t direction2) |
11 | 46 |
{ |
12 |
// reset_encoders (); |
|
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 |
|
|
13 | 55 |
for (uint8_t m=0; m<num_measurements; ++m) |
14 | 56 |
{ |
15 | 57 |
robot_set_motors (direction1, velocity, direction2, velocity); |
... | ... | |
23 | 66 |
|
24 | 67 |
// TODO instead of waiting a fixed time, wait until the wheels |
25 | 68 |
// are not moving any more, using the station's encoders. |
26 |
|
|
27 |
// send (turn motor on for time) |
|
28 |
// wait (done) |
|
29 |
// wait_stopped |
|
30 |
// send (read encoders) |
|
31 |
// receive (data) |
|
32 |
// read encoders |
|
33 |
|
|
69 |
|
|
70 |
encoders_read (&(data_station_l[m]), &(data_station_r[m])); |
|
71 |
//TODO robot_read_encoders () |
|
72 |
data_robot_l[m]=23; |
|
73 |
data_robot_r[m]=42; |
|
34 | 74 |
} |
35 | 75 |
|
36 |
// Send data |
|
37 |
for (uint8_t i=1; i<=2; ++i) |
|
38 |
{ |
|
39 |
uint8_t direction=(i==1)?direction1:direction2; |
|
40 |
|
|
41 |
if (direction!=motor_direction_off) |
|
42 |
{ |
|
43 |
usb_puts ("data encoder "); |
|
44 |
usb_puti (i); |
|
45 |
usb_puts (" "); |
|
46 |
usb_puts (motor_direction_string (direction)); |
|
47 |
usb_puts (" 100/100 200/200 300/300 400/400 500/500 600/600 700/700" NL); |
|
48 |
} |
|
49 |
} |
|
76 |
send_encoder_data (0, direction1, data_station_l, data_robot_l); |
|
77 |
send_encoder_data (1, direction2, data_station_r, data_robot_r); |
|
78 |
|
|
79 |
free (data_robot_r); |
|
80 |
free (data_robot_l); |
|
81 |
free (data_station_r); |
|
82 |
free (data_station_l); |
|
50 | 83 |
} |
51 | 84 |
|
85 |
|
|
86 |
// ********************** |
|
87 |
// ** Public functions ** |
|
88 |
// ********************** |
|
89 |
|
|
52 | 90 |
void test_encoder_all (void) |
53 | 91 |
{ |
54 | 92 |
usb_puts("# Testing encoders" NL); |
Also available in: Unified diff