root / trunk / code / projects / diagnostic_station / station / test_rangefinders.c @ 1310
History | View | Annotate | Download (1.95 KB)
1 | 1213 | deffi | #include <stdlib.h> |
---|---|---|---|
2 | |||
3 | 1151 | deffi | #include "test_rangefinders.h" |
4 | #include "global.h" |
||
5 | |||
6 | 1205 | deffi | #include "hardware.h" |
7 | |||
8 | 1182 | deffi | #include "comm_robot.h" |
9 | |||
10 | 1213 | deffi | #define wall_min 0 |
11 | #define wall_max 1000 |
||
12 | #define wall_inc 100 |
||
13 | 1202 | deffi | |
14 | 1213 | deffi | static uint16_t rangefinder_position (uint8_t num)
|
15 | { |
||
16 | switch (num)
|
||
17 | { |
||
18 | case 0: return 100; break; |
||
19 | case 1: return 200; break; |
||
20 | case 2: return 300; break; |
||
21 | case 3: return 400; break; |
||
22 | case 4: return 500; break; |
||
23 | } |
||
24 | |||
25 | return 0; |
||
26 | } |
||
27 | |||
28 | static void test_rangefinder_direction (uint8_t num, uint16_t wall_start, uint16_t wall_step, uint8_t num_steps, char *wall_direction_string) |
||
29 | { |
||
30 | // Allocate space for the data on the stack
|
||
31 | uint16_t *data_wall =malloc (num_steps*sizeof (uint16_t));
|
||
32 | uint16_t *data_rangefinder=malloc (num_steps*sizeof (uint16_t));
|
||
33 | |||
34 | // Rotate the robot to the correct position
|
||
35 | turntable_rotate_to_position (rangefinder_position (num)); |
||
36 | |||
37 | uint16_t wall=wall_start; |
||
38 | for (uint8_t i=0; i<num_steps; ++i) |
||
39 | { |
||
40 | wall_set_position (wall); |
||
41 | |||
42 | 1245 | deffi | data_wall[i]=wall_get_position (); |
43 | robot_read_rangefinder (num, &data_rangefinder[i]); |
||
44 | 1213 | deffi | |
45 | wall+=wall_step; |
||
46 | } |
||
47 | |||
48 | // Send data
|
||
49 | usb_puts ("data rangefinder ");
|
||
50 | usb_puti (num); |
||
51 | usb_puts (" ");
|
||
52 | usb_puts (wall_direction_string); |
||
53 | |||
54 | for (uint8_t i=0; i<num_steps; ++i) |
||
55 | { |
||
56 | usb_putc (' ');
|
||
57 | usb_puti (data_wall[i]); |
||
58 | usb_putc ('/');
|
||
59 | usb_puti (data_rangefinder[i]); |
||
60 | } |
||
61 | |||
62 | free (data_rangefinder); |
||
63 | free (data_wall); |
||
64 | |||
65 | usb_puts (NL); |
||
66 | } |
||
67 | |||
68 | |||
69 | 1202 | deffi | void test_rangefinder (uint8_t num)
|
70 | 1151 | deffi | { |
71 | 1202 | deffi | if (num>4) return; |
72 | 1151 | deffi | |
73 | 1202 | deffi | usb_puts("# Testing rangefinder ");
|
74 | usb_puti(num); |
||
75 | usb_puts(NL); |
||
76 | |||
77 | 1213 | deffi | test_rangefinder_direction (num, wall_min, wall_inc, 10, "out"); |
78 | test_rangefinder_direction (num, wall_max, -wall_inc, 10, "in"); |
||
79 | 1202 | deffi | } |
80 | 1151 | deffi | |
81 | 1202 | deffi | |
82 | void test_rangefinder_all (void) |
||
83 | { |
||
84 | usb_puts("# Testing rangefinders" NL);
|
||
85 | 1151 | deffi | |
86 | 1202 | deffi | for (int n=0; n<5; ++n) |
87 | test_rangefinder (n); |
||
88 | 1310 | bneuman | |
89 | |||
90 | turntable_rotate_to_position(TURNTABLE_HOME_POSITION); |
||
91 | 1166 | deffi | usb_puts("# Testing rangefinders finished" NL);
|
92 | 1151 | deffi | } |