Statistics
| Branch: | Tag: | Revision:

amiro-os / unittests / periphery-lld / src / ut_alld_a3906.c @ b309b751

History | View | Annotate | Download (12.8 KB)

1
/*
2
AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
3
Copyright (C) 2016..2018  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 <ut_alld_a3906.h>
20

    
21
#if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_USE_A3906)) || defined(__DOXYGEN__)
22

    
23
#include <aos_debug.h>
24
#include <chprintf.h>
25
#include <alld_a3906.h>
26
#include <aos_thread.h>
27
#include <stdlib.h>
28
#include <math.h>
29

    
30
/**
31
 * @brief   Interval to poll QEI in certain tests.
32
 */
33
#define QEI_POLL_INTERVAL_MS          100
34

    
35
/**
36
 * @brief   Threshold for QEI differences.
37
 * @details Differences smaller tahn or equal to this value are neglected (interpreted as zero).
38
 */
39
#define QEI_DIFF_THRESHOLD            4
40

    
41
/**
42
 * @brief   Enumerator to distinguish between left and right wheel.
43
 */
44
typedef enum {
45
  WHEEL_LEFT    = 0,  /**< left wheel identifier */
46
  WHEEL_RIGHT   = 1,  /**< right wheel identifier */
47
} wheel_t;
48

    
49
/**
50
 * @brief   Enumerator to distinguish directions.
51
 */
52
typedef enum {
53
  DIRECTION_FORWARD   = 0,  /**< forward direction identifier */
54
  DIRECTION_BACKWARD  = 1,  /**< backward direction identifier */
55
} direction_t;
56

    
57
/**
58
 * @brief   helper function to test each wheel and direction separately.
59
 *
60
 * @param[in] stream      Stream for input/output.
61
 * @param[in] data        Unit test meta data.
62
 * @param[in] wheel       Wheel to test.
63
 * @param[in] direction   Direction to test.
64
 * @param[in,out] result  Result variable to modify.
65
 */
66
void _wheelDirectionTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
67
{
68
  // local variables
69
  uint32_t status;
70
  bool qei_valid;
71
  apalQEICount_t qei_count[2];
72
  apalQEIDirection_t qei_direction;
73
  uint32_t timeout_counter;
74

    
75
  chprintf(stream, "%s wheel %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
76
  qei_valid = false;
77
  status = apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
78
  // increase PWM incrementally and read QEI data
79
  for (apalPWMwidth_t pwm_width = APAL_PWM_WIDTH_MIN; pwm_width < APAL_PWM_WIDTH_MAX; ++pwm_width) {
80
    status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
81
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
82
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
83
                                pwm_width);
84
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
85
    qei_valid = qei_valid || (qei_count[0] != qei_count[1]);
86
    aosThdUSleep(5 * MICROSECONDS_PER_SECOND / (APAL_PWM_WIDTH_MAX - APAL_PWM_WIDTH_MIN));
87
    qei_count[0] = qei_count[1];
88
  }
89
  status |= qei_valid ? 0x00 : 0x10;
90
  status |= apalQEIGetDirection((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_direction);
91
  status |= (qei_direction == ((direction == DIRECTION_FORWARD) ? APAL_QEI_DIRECTION_UP : APAL_QEI_DIRECTION_DOWN)) ? 0x00 : 0x20;
92

    
93
  // let the wheel spin free until it stops
94
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
95
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
96
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
97
                              0);
98
  qei_count[0] = 0;
99
  qei_count[1] = 0;
100
  timeout_counter = 0;
101
  do {
102
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
103
    aosThdMSleep(1);
104
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
105
    ++timeout_counter;
106
  } while ((qei_count[0] != qei_count[1]) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
107
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
108

    
109
  // report result
110
  if (status == APAL_STATUS_SUCCESS) {
111
    aosUtPassed(stream, result);
112
  } else {
113
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
114
  }
115

    
116
  return;
117
}
118

    
119
/**
120
 * @brief   A3905 unit test function.
121
 *
122
 * @param[in] stream  Stream for input/output.
123
 * @param[in] ut      Unit test object.
124
 *
125
 * @return            Unit test result value.
126
 */
127
aos_utresult_t utAlldA3906Func(BaseSequentialStream* stream, aos_unittest_t* ut)
128
{
129
  aosDbgCheck((ut->data != NULL) &&
130
              (((ut_a3906data_t*)ut->data)->driver != NULL) &&
131
              (((ut_a3906data_t*)ut->data)->pwm.driver != NULL) &&
132
              (((ut_a3906data_t*)ut->data)->qei.left != NULL) &&
133
              (((ut_a3906data_t*)ut->data)->qei.right != NULL));
134

    
135

    
136

    
137
  // local variables
138
  aos_utresult_t result = {0, 0};
139
  uint32_t status = 0;
140
  a3906_lld_power_t power_state;
141
  apalQEICount_t qei_count[2][2];
142
  apalQEICount_t qei_range[2];
143
  uint32_t timeout_counter;
144
  apalQEICount_t qei_increments[2][2];
145
  uint32_t stable_counter;
146
  apalQEICount_t qei_increments_diff[2];
147

    
148
  chprintf(stream, "enable power...\n");
149
  power_state = A3906_LLD_POWER_ON;
150
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
151
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
152
  status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00;
153
  if (status == APAL_STATUS_SUCCESS) {
154
    aosUtPassed(stream, &result);
155
  } else {
156
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
157
  }
158

    
159
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
160

    
161
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
162

    
163
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
164

    
165
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
166

    
167
  chprintf(stream, "both wheels full speed forward...\n");
168
  status = APAL_STATUS_SUCCESS;
169
  status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_MAX);
170
  status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_MAX);
171
  aosThdMSleep(100);
172
  qei_count[WHEEL_LEFT][0] = 0;
173
  qei_count[WHEEL_LEFT][1] = 0;
174
  qei_count[WHEEL_RIGHT][0] = 0;
175
  qei_count[WHEEL_RIGHT][1] = 0;
176
  status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.left, &qei_range[WHEEL_LEFT]);
177
  status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.right, &qei_range[WHEEL_RIGHT]);
178
  timeout_counter = 0;
179
  qei_increments[WHEEL_LEFT][0] = 0;
180
  qei_increments[WHEEL_LEFT][1] = 0;
181
  qei_increments[WHEEL_RIGHT][0] = 0;
182
  qei_increments[WHEEL_RIGHT][1] = 0;
183
  stable_counter = 0;
184
  do {
185
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
186
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
187
    aosThdMSleep(QEI_POLL_INTERVAL_MS);
188
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
189
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
190
    timeout_counter += QEI_POLL_INTERVAL_MS;
191
    qei_increments[WHEEL_LEFT][0] = qei_increments[WHEEL_LEFT][1];
192
    qei_increments[WHEEL_LEFT][1] = (qei_count[WHEEL_LEFT][1] > qei_count[WHEEL_LEFT][0]) ? (qei_count[WHEEL_LEFT][1] - qei_count[WHEEL_LEFT][0]) : (qei_count[WHEEL_LEFT][1] + (qei_range[WHEEL_LEFT] - qei_count[WHEEL_LEFT][0]));
193
    qei_increments[WHEEL_RIGHT][0] = qei_increments[WHEEL_RIGHT][1];
194
    qei_increments[WHEEL_RIGHT][1] = (qei_count[WHEEL_RIGHT][1] > qei_count[WHEEL_RIGHT][0]) ? (qei_count[WHEEL_RIGHT][1] - qei_count[WHEEL_RIGHT][0]) : (qei_count[WHEEL_RIGHT][1] + (qei_range[WHEEL_RIGHT] - qei_count[WHEEL_RIGHT][0]));
195
    qei_increments_diff[WHEEL_LEFT] = abs((int32_t)(qei_increments[WHEEL_LEFT][0]) - (int32_t)qei_increments[WHEEL_LEFT][1]);
196
    qei_increments_diff[WHEEL_RIGHT] = abs((int32_t)(qei_increments[WHEEL_RIGHT][0]) - (int32_t)qei_increments[WHEEL_RIGHT][1]);
197
    stable_counter = (qei_increments_diff[WHEEL_LEFT] <= QEI_DIFF_THRESHOLD && qei_increments_diff[WHEEL_RIGHT] < QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0;
198
  } while((stable_counter * QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout));
199
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
200
  if (status == APAL_STATUS_SUCCESS) {
201
    aosUtPassed(stream, &result);
202
    const float tps[2] = {qei_increments[WHEEL_LEFT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS),
203
                          qei_increments[WHEEL_RIGHT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS)};
204
    const float rpm[2] = {tps[WHEEL_LEFT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution),
205
                          tps[WHEEL_RIGHT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution)};
206
    const float velocity[2] = {tps[WHEEL_LEFT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1),
207
                               tps[WHEEL_RIGHT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1)};
208
    chprintf(stream, "left wheel:\n");
209
    chprintf(stream, "\t%f tps\n", tps[WHEEL_LEFT]);
210
    chprintf(stream, "\t%f RPM\n", rpm[WHEEL_LEFT]);
211
    chprintf(stream, "\t%f m/s\n", velocity[WHEEL_LEFT]);
212
    chprintf(stream, "right wheel:\n");
213
    chprintf(stream, "\t%f tps\n", tps[WHEEL_RIGHT]);
214
    chprintf(stream, "\t%f RPM\n", rpm[WHEEL_RIGHT]);
215
    chprintf(stream, "\t%f m/s\n", velocity[WHEEL_RIGHT]);
216
    chprintf(stream, "\n");
217
  } else {
218
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
219
  }
220

    
221
  chprintf(stream, "disable power... \n");
222
  power_state = A3906_LLD_POWER_OFF;
223
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
224
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
225
  status |= (power_state != A3906_LLD_POWER_OFF) ? 0x10 : 0x00;
226
  qei_count[WHEEL_LEFT][0] = 0;
227
  qei_count[WHEEL_LEFT][1] = 0;
228
  qei_count[WHEEL_RIGHT][0] = 0;
229
  qei_count[WHEEL_RIGHT][1] = 0;
230
  timeout_counter = 0;
231
  stable_counter = 0;
232
  do {
233
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
234
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
235
    aosThdMSleep(1);
236
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
237
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
238
    ++timeout_counter;
239
    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;
240
  } while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout));
241
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
242
  if (status == APAL_STATUS_SUCCESS) {
243
    aosUtPassed(stream, &result);
244
  } else {
245
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
246
  }
247

    
248
  // stop the PWM
249
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
250
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
251
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
252
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
253

    
254
  aosUtInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver));
255

    
256
  return result;
257
}
258

    
259
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_USE_A3906) */