Skip to content

Commit 6019de5

Browse files
laurensvalklaurensvalk
committed
pbio/test/drivebase: Add drivebase tests.
1 parent 1895d49 commit 6019de5

File tree

3 files changed

+155
-0
lines changed

3 files changed

+155
-0
lines changed

‎.vscode/launch.json‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -221,6 +221,7 @@
221221
"src/color/test_color_to_hsv",
222222
"src/color/test_color_to_rgb",
223223
"src/color/test_color_hsv_compression",
224+
"src/drivebase/test_drivebase_basics",
224225
"src/light/test_light_animation",
225226
"src/light/test_color_light",
226227
"src/light/test_light_matrix",

‎lib/pbio/test/src/test_drivebase.c‎

Lines changed: 152 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
// SPDX-License-Identifier: MIT
2+
// Copyright (c) 2020-2022 The Pybricks Authors
3+
4+
#include <errno.h>
5+
#include <signal.h>
6+
#include <stdint.h>
7+
#include <stdio.h>
8+
#include <stdlib.h>
9+
#include <string.h>
10+
#include <unistd.h>
11+
#include <sys/types.h>
12+
#include <sys/wait.h>
13+
14+
#include <contiki.h>
15+
#include <tinytest.h>
16+
#include <tinytest_macros.h>
17+
18+
#include <pbdrv/motor_driver.h>
19+
#include <pbio/angle.h>
20+
#include <pbio/control.h>
21+
#include <pbio/drivebase.h>
22+
#include <pbio/error.h>
23+
#include <pbio/logger.h>
24+
#include <pbio/int_math.h>
25+
#include <pbio/motor_process.h>
26+
#include <pbio/servo.h>
27+
#include <test-pbio.h>
28+
29+
#include "../src/processes.h"
30+
#include "../drv/core.h"
31+
#include "../drv/clock/clock_test.h"
32+
#include "../drv/motor_driver/motor_driver_virtual_simulation.h"
33+
34+
static PT_THREAD(test_drivebase_basics(struct pt *pt)) {
35+
36+
static struct timer timer;
37+
38+
static pbio_servo_t *srv_left;
39+
static pbio_servo_t *srv_right;
40+
static pbio_drivebase_t *db;
41+
42+
static int32_t drive_distance;
43+
static int32_t drive_speed;
44+
static int32_t drive_acceleration;
45+
static int32_t drive_deceleration;
46+
static int32_t turn_angle_start;
47+
static int32_t turn_angle;
48+
static int32_t turn_rate;
49+
static int32_t turn_acceleration;
50+
static int32_t turn_deceleration;
51+
52+
static bool stalled;
53+
static uint32_t stall_duration;
54+
55+
// Start motor driver simulation process.
56+
pbdrv_motor_driver_init_manual();
57+
58+
PT_BEGIN(pt);
59+
60+
// Wait for motor simulation process to be ready.
61+
while (pbdrv_init_busy()) {
62+
PT_YIELD(pt);
63+
}
64+
65+
// Start motor control process manually.
66+
pbio_motor_process_start();
67+
68+
// Initialize the servos.
69+
tt_uint_op(pbio_servo_get_servo(PBIO_PORT_ID_A, &srv_left), ==, PBIO_SUCCESS);
70+
tt_uint_op(pbio_servo_setup(srv_left, PBIO_DIRECTION_COUNTERCLOCKWISE, 1000, true, 0), ==, PBIO_SUCCESS);
71+
tt_uint_op(pbio_servo_get_servo(PBIO_PORT_ID_B, &srv_right), ==, PBIO_SUCCESS);
72+
tt_uint_op(pbio_servo_setup(srv_right, PBIO_DIRECTION_CLOCKWISE, 1000, true, 0), ==, PBIO_SUCCESS);
73+
74+
// Set up the drivebase.
75+
tt_uint_op(pbio_drivebase_get_drivebase(&db, srv_left, srv_right, 56000, 112000), ==, PBIO_SUCCESS);
76+
tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle_start, &turn_rate), ==, PBIO_SUCCESS);
77+
tt_uint_op(pbio_drivebase_is_stalled(db, &stalled, &stall_duration), ==, PBIO_SUCCESS);
78+
tt_want(!stalled);
79+
80+
// Get current settings and change them.
81+
tt_uint_op(pbio_drivebase_get_drive_settings(db,
82+
&drive_speed,
83+
&drive_acceleration,
84+
&drive_deceleration,
85+
&turn_rate,
86+
&turn_acceleration,
87+
&turn_deceleration), ==, PBIO_SUCCESS);
88+
89+
// Try to set invalid settings.
90+
tt_uint_op(pbio_drivebase_set_drive_settings(db,
91+
drive_speed,
92+
drive_acceleration * 100,
93+
drive_deceleration,
94+
turn_rate,
95+
turn_acceleration,
96+
turn_deceleration), ==, PBIO_ERROR_INVALID_ARG);
97+
98+
// Try to set invalid settings.
99+
tt_uint_op(pbio_drivebase_set_drive_settings(db,
100+
200,
101+
drive_acceleration * 2,
102+
drive_deceleration,
103+
turn_rate,
104+
turn_acceleration,
105+
turn_deceleration), ==, PBIO_SUCCESS);
106+
107+
// Drive straight for a distance and coast smart.
108+
tt_uint_op(pbio_drivebase_drive_straight(db, 1000, PBIO_CONTROL_ON_COMPLETION_COAST_SMART), ==, PBIO_SUCCESS);
109+
pbio_test_sleep_until(pbio_drivebase_is_done(db));
110+
111+
// Target should be stationary and close to target.
112+
pbio_test_sleep_ms(&timer, 200);
113+
tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle, &turn_rate), ==, PBIO_SUCCESS);
114+
tt_want(pbio_test_int_is_close(drive_distance, 1000, 30));
115+
tt_want(pbio_test_int_is_close(drive_speed, 0, 50));
116+
tt_want(pbio_test_int_is_close(turn_angle, turn_angle_start, 5));
117+
tt_want(pbio_test_int_is_close(turn_rate, 0, 10));
118+
119+
// Drive straight for a distance and keep driving.
120+
tt_uint_op(pbio_drivebase_drive_straight(db, 1000, PBIO_CONTROL_ON_COMPLETION_CONTINUE), ==, PBIO_SUCCESS);
121+
pbio_test_sleep_until(pbio_drivebase_is_done(db));
122+
123+
// Target should be moving at given speed and close to target.
124+
tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle, &turn_rate), ==, PBIO_SUCCESS);
125+
tt_want(pbio_test_int_is_close(drive_distance, 2000, 20));
126+
tt_want(pbio_test_int_is_close(drive_speed, 200, 5));
127+
tt_want(pbio_test_int_is_close(turn_angle, turn_angle_start, 5));
128+
tt_want(pbio_test_int_is_close(turn_rate, 0, 5));
129+
130+
// Test driving/turning forever, maintaining the speed we are already on.
131+
tt_uint_op(pbio_drivebase_drive_forever(db, 200, 90), ==, PBIO_SUCCESS);
132+
tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle, &turn_rate), ==, PBIO_SUCCESS);
133+
tt_want(pbio_test_int_is_close(drive_distance, 2000, 20));
134+
tt_want(pbio_test_int_is_close(drive_speed, 200, 5));
135+
tt_want(pbio_test_int_is_close(turn_angle, turn_angle_start, 5));
136+
tt_want(pbio_test_int_is_close(turn_rate, 0, 5));
137+
138+
// After a while, the target speed/rate should be reached.
139+
pbio_test_sleep_ms(&timer, 2000);
140+
tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle, &turn_rate), ==, PBIO_SUCCESS);
141+
tt_want(pbio_test_int_is_close(drive_speed, 200, 5));
142+
tt_want(pbio_test_int_is_close(turn_rate, 90, 5));
143+
144+
end:
145+
146+
PT_END(pt);
147+
}
148+
149+
struct testcase_t pbio_drivebase_tests[] = {
150+
PBIO_PT_THREAD_TEST(test_drivebase_basics),
151+
END_OF_TESTCASES
152+
};

‎lib/pbio/test/test-pbio.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,7 @@ extern struct testcase_t pbdrv_pwm_tests[];
8282
extern struct testcase_t pbio_angle_tests[];
8383
extern struct testcase_t pbio_battery_tests[];
8484
extern struct testcase_t pbio_color_tests[];
85+
extern struct testcase_t pbio_drivebase_tests[];
8586
extern struct testcase_t pbio_light_animation_tests[];
8687
extern struct testcase_t pbio_color_light_tests[];
8788
extern struct testcase_t pbio_light_matrix_tests[];
@@ -100,6 +101,7 @@ static struct testgroup_t test_groups[] = {
100101
{ "src/angle/", pbio_angle_tests },
101102
{ "src/battery/", pbio_battery_tests },
102103
{ "src/color/", pbio_color_tests },
104+
{ "src/drivebase/", pbio_drivebase_tests },
103105
{ "src/light/", pbio_light_animation_tests },
104106
{ "src/light/", pbio_color_light_tests },
105107
{ "src/light/", pbio_light_matrix_tests },

0 commit comments

Comments
 (0)