Statistics
| Branch: | Tag: | Revision:

amiro-os / test / periphery-lld / A3906_v1 / aos_test_A3906.c @ b23ca7cc

History | View | Annotate | Download (14.232 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
#include <aos_test_A3906.h>
21

    
22
#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
23

    
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        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, aos_test_a3906data_t* data, wheel_t wheel, direction_t direction, aos_testresult_t* result)
86
{
87
  // local variables
88
  int32_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
    aosTestPassed(stream, result);
131
  } else {
132
    aosTestFailedMsg(stream, result, "0x%08X\n", status);
133
  }
134

    
135
  return;
136
}
137

    
138
void _wheelSpeedTest(BaseSequentialStream* stream, aos_test_a3906data_t* data, wheel_t wheel, direction_t direction, aos_testresult_t* result)
139
{
140
  // local variables
141
  int32_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
    aosTestPassed(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
    aosTestFailedMsg(stream, result, "0x%08X\n", status);
192
  }
193
}
194

    
195
/******************************************************************************/
196
/* EXPORTED FUNCTIONS                                                         */
197
/******************************************************************************/
198

    
199
/**
200
 * @brief   A3905 test function.
201
 *
202
 * @param[in] stream  Stream for input/output.
203
 * @param[in] test    Test object.
204
 *
205
 * @return            Test result value.
206
 */
207
aos_testresult_t aosTestA3906Func(BaseSequentialStream* stream, const aos_test_t* test)
208
{
209
  aosDbgCheck((test->data != NULL) &&
210
              (((aos_test_a3906data_t*)test->data)->driver != NULL) &&
211
              (((aos_test_a3906data_t*)test->data)->pwm.driver != NULL) &&
212
              (((aos_test_a3906data_t*)test->data)->qei.left != NULL) &&
213
              (((aos_test_a3906data_t*)test->data)->qei.right != NULL));
214

    
215

    
216

    
217
  // local variables
218
  aos_testresult_t result = {0, 0};
219
  int32_t status;
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(((aos_test_a3906data_t*)test->data)->driver, power_state);
228
  status |= a3906_lld_get_power(((aos_test_a3906data_t*)test->data)->driver, &power_state);
229
  status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00;
230
  if (status == APAL_STATUS_SUCCESS) {
231
    aosTestPassed(stream, &result);
232
  } else {
233
    aosTestFailedMsg(stream, &result, "0x%08X\n", status);
234
  }
235

    
236
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
237

    
238
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
239

    
240
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
241

    
242
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
243

    
244
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
245

    
246
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
247

    
248
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
249

    
250
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->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(((aos_test_a3906data_t*)test->data)->driver, power_state);
255
  status |= a3906_lld_get_power(((aos_test_a3906data_t*)test->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(((aos_test_a3906data_t*)test->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
265
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
266
    aosThdMSleep(1);
267
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
268
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->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 <= ((aos_test_a3906data_t*)test->data)->timeout));
272
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((aos_test_a3906data_t*)test->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
273
  if (status == APAL_STATUS_SUCCESS) {
274
    aosTestPassed(stream, &result);
275
  } else {
276
    aosTestFailedMsg(stream, &result, "0x%08X\n", status);
277
  }
278

    
279
  // stop the PWM
280
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
281
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
282
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
283
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
284

    
285
  aosTestInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver));
286

    
287
  return result;
288
}
289

    
290
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */