amiro-os / unittests / periphery-lld / src / ut_alld_A3906_v1.c @ ddf34c3d
History | View | Annotate | Download (14.1 KB)
1 |
/*
|
---|---|
2 |
AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
|
3 |
Copyright (C) 2016..2019 Thomas Schöpping et al.
|
4 |
|
5 |
This program is free software: you can redistribute it and/or modify
|
6 |
it under the terms of the GNU General Public License as published by
|
7 |
the Free Software Foundation, either version 3 of the License, or
|
8 |
(at your option) any later version.
|
9 |
|
10 |
This program is distributed in the hope that it will be useful,
|
11 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
12 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
13 |
GNU General Public License for more details.
|
14 |
|
15 |
You should have received a copy of the GNU General Public License
|
16 |
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
17 |
*/
|
18 |
|
19 |
#include <amiroos.h> |
20 |
|
21 |
#if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_A3906) && (AMIROLLD_CFG_A3906 == 1)) || defined(__DOXYGEN__) |
22 |
|
23 |
#include <ut_alld_A3906_v1.h> |
24 |
#include <stdlib.h> |
25 |
#include <math.h> |
26 |
|
27 |
/******************************************************************************/
|
28 |
/* LOCAL DEFINITIONS */
|
29 |
/******************************************************************************/
|
30 |
|
31 |
/**
|
32 |
* @brief Interval to poll QEI in certain tests.
|
33 |
*/
|
34 |
#define QEI_POLL_INTERVAL_MS 100 |
35 |
|
36 |
/**
|
37 |
* @brief Threshold for QEI differences.
|
38 |
* @details Differences smaller than or equal to this value are neglected (interpreted as zero).
|
39 |
* The value can be interpreted as encoder ticks per second (tps).
|
40 |
* @note The expected value is about 7000 tps and a jitter of up to ±2% is ok.
|
41 |
*/
|
42 |
#define QEI_DIFF_THRESHOLD (apalQEICount_t)(7000 * 0.02f) |
43 |
|
44 |
/******************************************************************************/
|
45 |
/* EXPORTED VARIABLES */
|
46 |
/******************************************************************************/
|
47 |
|
48 |
/******************************************************************************/
|
49 |
/* LOCAL TYPES */
|
50 |
/******************************************************************************/
|
51 |
|
52 |
/**
|
53 |
* @brief Enumerator to distinguish between left and right wheel.
|
54 |
*/
|
55 |
typedef enum { |
56 |
WHEEL_LEFT = 0, /**< left wheel identifier */ |
57 |
WHEEL_RIGHT = 1, /**< right wheel identifier */ |
58 |
} wheel_t; |
59 |
|
60 |
/**
|
61 |
* @brief Enumerator to distinguish directions.
|
62 |
*/
|
63 |
typedef enum { |
64 |
DIRECTION_FORWARD = 0, /**< forward direction identifier */ |
65 |
DIRECTION_BACKWARD = 1, /**< backward direction identifier */ |
66 |
} direction_t; |
67 |
|
68 |
/******************************************************************************/
|
69 |
/* LOCAL VARIABLES */
|
70 |
/******************************************************************************/
|
71 |
|
72 |
/******************************************************************************/
|
73 |
/* LOCAL FUNCTIONS */
|
74 |
/******************************************************************************/
|
75 |
|
76 |
/**
|
77 |
* @brief helper function to test each wheel and direction separately.
|
78 |
*
|
79 |
* @param[in] stream Stream for input/output.
|
80 |
* @param[in] data Unit test meta data.
|
81 |
* @param[in] wheel Wheel to test.
|
82 |
* @param[in] direction Direction to test.
|
83 |
* @param[in,out] result Result variable to modify.
|
84 |
*/
|
85 |
void _wheelDirectionTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
|
86 |
{ |
87 |
// local variables
|
88 |
uint32_t status; |
89 |
bool qei_valid;
|
90 |
apalQEICount_t qei_count[2];
|
91 |
apalQEIDirection_t qei_direction; |
92 |
uint32_t timeout_counter; |
93 |
|
94 |
chprintf(stream, "%s wheel %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward"); |
95 |
qei_valid = false;
|
96 |
status = apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
|
97 |
// increase PWM incrementally and read QEI data
|
98 |
for (apalPWMwidth_t pwm_width = APAL_PWM_WIDTH_MIN; pwm_width < APAL_PWM_WIDTH_MAX; ++pwm_width) {
|
99 |
status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
100 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) : |
101 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward), |
102 |
pwm_width); |
103 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
|
104 |
qei_valid = qei_valid || (qei_count[0] != qei_count[1]); |
105 |
aosThdUSleep(5 * MICROSECONDS_PER_SECOND / (APAL_PWM_WIDTH_MAX - APAL_PWM_WIDTH_MIN));
|
106 |
qei_count[0] = qei_count[1]; |
107 |
} |
108 |
status |= qei_valid ? 0x00 : 0x10; |
109 |
status |= apalQEIGetDirection((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_direction); |
110 |
status |= (qei_direction == ((direction == DIRECTION_FORWARD) ? APAL_QEI_DIRECTION_UP : APAL_QEI_DIRECTION_DOWN)) ? 0x00 : 0x20; |
111 |
|
112 |
// let the wheel spin free until it stops
|
113 |
status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
114 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) : |
115 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward), |
116 |
0);
|
117 |
qei_count[0] = 0; |
118 |
qei_count[1] = 0; |
119 |
timeout_counter = 0;
|
120 |
do {
|
121 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
|
122 |
aosThdMSleep(1);
|
123 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
|
124 |
++timeout_counter; |
125 |
} while ((qei_count[0] != qei_count[1]) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout)); |
126 |
status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
|
127 |
|
128 |
// report result
|
129 |
if (status == APAL_STATUS_SUCCESS) {
|
130 |
aosUtPassed(stream, result); |
131 |
} else {
|
132 |
aosUtFailedMsg(stream, result, "0x%08X\n", status);
|
133 |
} |
134 |
|
135 |
return;
|
136 |
} |
137 |
|
138 |
void _wheelSpeedTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
|
139 |
{ |
140 |
// local variables
|
141 |
uint32_t status; |
142 |
apalQEICount_t qei_range; |
143 |
apalQEICount_t qei_count[2] = {0}; |
144 |
apalQEICount_t qei_increments[2] = {0}; |
145 |
apalQEICount_t qei_increments_diff = 0;
|
146 |
uint32_t timeout_counter = 0;
|
147 |
uint32_t stable_counter = 0;
|
148 |
|
149 |
chprintf(stream, "%s wheel full speed %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward"); |
150 |
// spin up the wheel with full speed
|
151 |
status = apalQEIGetRange((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_range); |
152 |
status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
153 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) : |
154 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward), |
155 |
APAL_PWM_WIDTH_MAX); |
156 |
aosThdMSleep(100);
|
157 |
do {
|
158 |
// read QEI data to determine speed
|
159 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
|
160 |
aosThdMSleep(QEI_POLL_INTERVAL_MS); |
161 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
|
162 |
timeout_counter += QEI_POLL_INTERVAL_MS; |
163 |
qei_increments[0] = qei_increments[1]; |
164 |
qei_increments[1] = (direction == DIRECTION_FORWARD) ?
|
165 |
((qei_count[1] > qei_count[0]) ? (qei_count[1] - qei_count[0]) : (qei_count[1] + (qei_range - qei_count[0]))) : |
166 |
((qei_count[0] > qei_count[1]) ? (qei_count[0] - qei_count[1]) : (qei_count[0] + (qei_range - qei_count[1]))); |
167 |
qei_increments_diff = abs((int32_t)qei_increments[0] - (int32_t)qei_increments[1]) * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS); |
168 |
stable_counter = ((qei_increments[0] != 0 || qei_increments[1] != 0) && qei_increments_diff <= QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0; |
169 |
if (qei_increments[0] != 0 && stable_counter == 0) { |
170 |
chprintf(stream, "\tunstable speed? jitter of %u tps is above threshold (%u tps).\n", qei_increments_diff, QEI_DIFF_THRESHOLD);
|
171 |
} |
172 |
} while ((stable_counter* QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
|
173 |
status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
174 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) : |
175 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward), |
176 |
APAL_PWM_WIDTH_OFF); |
177 |
status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
|
178 |
|
179 |
// report results
|
180 |
if (status == APAL_STATUS_SUCCESS) {
|
181 |
aosUtPassed(stream, result); |
182 |
const float tps = qei_increments[1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS); |
183 |
const float rpm = tps * SECONDS_PER_MINUTE / (float)data->qei.increments_per_revolution; |
184 |
const float velocity = tps / (float)data->qei.increments_per_revolution * ((wheel == WHEEL_LEFT) ? data->wheel_diameter.left : data->wheel_diameter.right) * acos(-1); |
185 |
chprintf(stream, "\t%f tps\n", tps);
|
186 |
chprintf(stream, "\t%f RPM\n", rpm);
|
187 |
chprintf(stream, "\t%f m/s\n", velocity);
|
188 |
chprintf(stream, "\n");
|
189 |
} |
190 |
else {
|
191 |
aosUtFailedMsg(stream, result, "0x%08X\n", status);
|
192 |
} |
193 |
} |
194 |
|
195 |
/******************************************************************************/
|
196 |
/* EXPORTED FUNCTIONS */
|
197 |
/******************************************************************************/
|
198 |
|
199 |
/**
|
200 |
* @brief A3905 unit test function.
|
201 |
*
|
202 |
* @param[in] stream Stream for input/output.
|
203 |
* @param[in] ut Unit test object.
|
204 |
*
|
205 |
* @return Unit test result value.
|
206 |
*/
|
207 |
aos_utresult_t utAlldA3906Func(BaseSequentialStream* stream, aos_unittest_t* ut) |
208 |
{ |
209 |
aosDbgCheck((ut->data != NULL) &&
|
210 |
(((ut_a3906data_t*)ut->data)->driver != NULL) &&
|
211 |
(((ut_a3906data_t*)ut->data)->pwm.driver != NULL) &&
|
212 |
(((ut_a3906data_t*)ut->data)->qei.left != NULL) &&
|
213 |
(((ut_a3906data_t*)ut->data)->qei.right != NULL));
|
214 |
|
215 |
|
216 |
|
217 |
// local variables
|
218 |
aos_utresult_t result = {0, 0}; |
219 |
uint32_t status = 0;
|
220 |
a3906_lld_power_t power_state; |
221 |
apalQEICount_t qei_count[2][2]; |
222 |
uint32_t timeout_counter; |
223 |
uint32_t stable_counter; |
224 |
|
225 |
chprintf(stream, "enable power...\n");
|
226 |
power_state = A3906_LLD_POWER_ON; |
227 |
status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state); |
228 |
status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state); |
229 |
status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00; |
230 |
if (status == APAL_STATUS_SUCCESS) {
|
231 |
aosUtPassed(stream, &result); |
232 |
} else {
|
233 |
aosUtFailedMsg(stream, &result, "0x%08X\n", status);
|
234 |
} |
235 |
|
236 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
237 |
|
238 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
239 |
|
240 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
241 |
|
242 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
243 |
|
244 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
245 |
|
246 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
247 |
|
248 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
249 |
|
250 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
251 |
|
252 |
chprintf(stream, "disable power... \n");
|
253 |
power_state = A3906_LLD_POWER_OFF; |
254 |
status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state); |
255 |
status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state); |
256 |
status |= (power_state != A3906_LLD_POWER_OFF) ? 0x10 : 0x00; |
257 |
qei_count[WHEEL_LEFT][0] = 0; |
258 |
qei_count[WHEEL_LEFT][1] = 0; |
259 |
qei_count[WHEEL_RIGHT][0] = 0; |
260 |
qei_count[WHEEL_RIGHT][1] = 0; |
261 |
timeout_counter = 0;
|
262 |
stable_counter = 0;
|
263 |
do {
|
264 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
|
265 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
|
266 |
aosThdMSleep(1);
|
267 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
|
268 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
|
269 |
++timeout_counter; |
270 |
stable_counter = (qei_count[WHEEL_LEFT][0] == qei_count[WHEEL_LEFT][1] && qei_count[WHEEL_RIGHT][0] == qei_count[WHEEL_RIGHT][1]) ? stable_counter+1 : 0; |
271 |
} while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout)); |
272 |
status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
|
273 |
if (status == APAL_STATUS_SUCCESS) {
|
274 |
aosUtPassed(stream, &result); |
275 |
} else {
|
276 |
aosUtFailedMsg(stream, &result, "0x%08X\n", status);
|
277 |
} |
278 |
|
279 |
// stop the PWM
|
280 |
a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF); |
281 |
a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF); |
282 |
a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF); |
283 |
a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF); |
284 |
|
285 |
aosUtInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver)); |
286 |
|
287 |
return result;
|
288 |
} |
289 |
|
290 |
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_A3906) && (AMIROLLD_CFG_A3906 == 1) */ |