root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1202
History | View | Annotate | Download (1.49 KB)
1 |
#include "test_motors.h" |
---|---|
2 |
#include "global.h" |
3 |
|
4 |
#include "comm_robot.h" |
5 |
|
6 |
// NB don't produce overflows!
|
7 |
#define vel_min 160 |
8 |
#define vel_max 250 |
9 |
#define vel_inc 10 |
10 |
#define vel_delay 200 |
11 |
|
12 |
static void test_motors_direction_velocity (uint8_t direction1, uint8_t direction2, uint8_t velocity) |
13 |
{ |
14 |
robot_set_motors (direction1, velocity, direction2, velocity); |
15 |
delay_ms (vel_delay); |
16 |
|
17 |
// wait (steady)
|
18 |
//
|
19 |
// reset_encoders ();
|
20 |
// for (measurements)
|
21 |
// {
|
22 |
// measure left/right
|
23 |
// wait
|
24 |
// }
|
25 |
} |
26 |
|
27 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
28 |
{ |
29 |
// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4
|
30 |
// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is
|
31 |
// easy and performance is not an issue here anyway.
|
32 |
|
33 |
for (uint16_t vel=vel_min; vel<=vel_max; vel+=vel_inc)
|
34 |
test_motors_direction_velocity (direction1, direction2, vel); |
35 |
|
36 |
for (uint16_t vel=vel_max; vel>=vel_min; vel-=vel_inc)
|
37 |
test_motors_direction_velocity (direction1, direction2, vel); |
38 |
} |
39 |
|
40 |
void test_motor_all (void) |
41 |
{ |
42 |
usb_puts("# Testing motors" NL);
|
43 |
|
44 |
test_motors_direction (FORWARD, BACKWARD); |
45 |
test_motors_direction (BACKWARD, FORWARD); |
46 |
|
47 |
robot_set_motors_off (); |
48 |
|
49 |
//send data
|
50 |
|
51 |
|
52 |
usb_puts("# Testing motors finished" NL);
|
53 |
} |
54 |
|
55 |
void test_motor (uint8_t num)
|
56 |
{ |
57 |
// TODO implement single motor testing
|
58 |
if (!(num==1 || num==2)) return; |
59 |
|
60 |
test_motor_all (); |
61 |
} |