root / trunk / code / projects / diagnostic_station / station / test_bom.c @ 1390
History | View | Annotate | Download (3.35 KB)
1 | 1151 | deffi | #include "test_bom.h" |
---|---|---|---|
2 | #include "global.h" |
||
3 | |||
4 | 1212 | deffi | #include "hardware_turntable.h" |
5 | 1254 | deffi | #include "hardware_ibom.h" |
6 | 1212 | deffi | #include "comm_robot.h" |
7 | |||
8 | static uint16_t bom_emitter_position (uint8_t num)
|
||
9 | { |
||
10 | 1292 | deffi | // 0 is the robot facing the station (BOM #4 aligned with IBOM).
|
11 | // Positive is counter-clockwise on the bom (robot turning clockwise)
|
||
12 | |||
13 | // The difference in angle between two positions. 1024 steps on a full turn, 16 emitters/detectors.
|
||
14 | #define increment 1024/16 |
||
15 | |||
16 | 1212 | deffi | switch (num)
|
17 | { |
||
18 | 1292 | deffi | case 0: return (-4)*increment; break; |
19 | case 1: return (-3)*increment; break; |
||
20 | case 2: return (-2)*increment; break; |
||
21 | case 3: return (-1)*increment; break; |
||
22 | case 4: return ( 0)*increment; break; |
||
23 | case 5: return ( 1)*increment; break; |
||
24 | case 6: return ( 2)*increment; break; |
||
25 | case 7: return ( 3)*increment; break; |
||
26 | case 8: return ( 4)*increment; break; |
||
27 | case 9: return ( 5)*increment; break; |
||
28 | case 10: return ( 6)*increment; break; |
||
29 | case 11: return ( 7)*increment; break; |
||
30 | case 12: return ( 8)*increment; break; |
||
31 | case 13: return ( 9)*increment; break; |
||
32 | case 14: return (10)*increment; break; |
||
33 | case 15: return (11)*increment; break; |
||
34 | 1212 | deffi | default: return 0; break; |
35 | } |
||
36 | 1292 | deffi | #undef increment
|
37 | 1212 | deffi | } |
38 | |||
39 | static uint16_t bom_detector_position (uint8_t num)
|
||
40 | { |
||
41 | 1292 | deffi | // Austin sez: detectors and emitters are supposed to be parallel.
|
42 | return bom_emitter_position (num);
|
||
43 | 1212 | deffi | } |
44 | |||
45 | |||
46 | 1202 | deffi | void test_bom_bitmask (uint16_t emitter_bitmask, uint16_t detector_bitmask)
|
47 | 1151 | deffi | { |
48 | 1166 | deffi | usb_puts("# Testing BOM" NL);
|
49 | 1151 | deffi | |
50 | 1202 | deffi | for (uint8_t n=0; n<16; ++n, emitter_bitmask>>=1, detector_bitmask>>=1) |
51 | 1151 | deffi | { |
52 | 1202 | deffi | if (emitter_bitmask&1) |
53 | 1151 | deffi | { |
54 | 1244 | deffi | uint16_t values[3];
|
55 | 1212 | deffi | |
56 | 1166 | deffi | usb_puts("# Testing BOM emitter ");
|
57 | 1151 | deffi | usb_puti(n); |
58 | usb_puts(NL); |
||
59 | 1212 | deffi | |
60 | turntable_rotate_to_position (bom_emitter_position (n)); |
||
61 | 1220 | deffi | robot_set_bom (1<<n);
|
62 | 1212 | deffi | |
63 | 1254 | deffi | ibom_update (); |
64 | 1244 | deffi | #define TOP 0 |
65 | #define LEFT 1 |
||
66 | #define RIGHT 2 |
||
67 | 1254 | deffi | ibom_read (&values[TOP], &values[LEFT], &values[RIGHT]); |
68 | 1212 | deffi | |
69 | 1220 | deffi | robot_set_bom (0);
|
70 | 1212 | deffi | |
71 | // Send data
|
||
72 | usb_puts ("data bom emitter");
|
||
73 | 1244 | deffi | usb_puts (" top/" ); usb_puti (values[TOP ]);
|
74 | usb_puts (" left/" ); usb_puti (values[LEFT ]);
|
||
75 | usb_puts (" right/"); usb_puti (values[RIGHT]);
|
||
76 | #undef TOP
|
||
77 | #undef LEFT
|
||
78 | #undef RIGHT
|
||
79 | 1203 | deffi | usb_puts (NL); |
80 | 1151 | deffi | } |
81 | |||
82 | 1202 | deffi | if (detector_bitmask&1) |
83 | 1151 | deffi | { |
84 | 1166 | deffi | usb_puts("# Testing BOM detector ");
|
85 | 1151 | deffi | usb_puti(n); |
86 | usb_puts(NL); |
||
87 | 1212 | deffi | turntable_rotate_to_position (bom_detector_position (n)); |
88 | |||
89 | 1245 | deffi | uint16_t on_value, off_value; |
90 | |||
91 | 1254 | deffi | ibom_set (true);
|
92 | 1245 | deffi | robot_read_bom (n, &on_value); |
93 | 1254 | deffi | ibom_set (false);
|
94 | 1245 | deffi | robot_read_bom (n, &off_value); |
95 | 1212 | deffi | |
96 | // Send data
|
||
97 | 1203 | deffi | usb_puts ("data bom detector ");
|
98 | usb_puti (n); |
||
99 | 1212 | deffi | usb_puts (" on/");
|
100 | usb_puti (on_value); |
||
101 | usb_puts (" off/");
|
||
102 | usb_puti (off_value); |
||
103 | 1203 | deffi | usb_puts (NL); |
104 | 1151 | deffi | } |
105 | } |
||
106 | |||
107 | 1309 | bneuman | turntable_rotate_to_position(TURNTABLE_HOME_POSITION); |
108 | 1166 | deffi | usb_puts("# Testing BOM finished" NL);
|
109 | 1151 | deffi | } |
110 | 1202 | deffi | |
111 | void test_bom (uint8_t num, bool test_emitter, bool test_detector) |
||
112 | { |
||
113 | uint16_t emitter_bitmask=0, detector_bitmask=0; |
||
114 | |||
115 | if (test_emitter && num<16) emitter_bitmask =1<<num; |
||
116 | if (test_detector && num<16) detector_bitmask=1<<num; |
||
117 | |||
118 | test_bom_bitmask (emitter_bitmask, detector_bitmask); |
||
119 | } |
||
120 | |||
121 | void test_bom_all (bool test_emitters, bool test_detectors) |
||
122 | { |
||
123 | uint16_t emitter_bitmask=0, detector_bitmask=0; |
||
124 | |||
125 | if (test_emitters ) emitter_bitmask =0xFFFF; |
||
126 | if (test_detectors) detector_bitmask=0xFFFF; |
||
127 | |||
128 | test_bom_bitmask (emitter_bitmask, detector_bitmask); |
||
129 | } |