root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1208
History | View | Annotate | Download (2.83 KB)
1 | 1151 | deffi | #include "test_motors.h" |
---|---|---|---|
2 | #include "global.h" |
||
3 | |||
4 | 1182 | deffi | #include "comm_robot.h" |
5 | 1159 | deffi | |
6 | // NB don't produce overflows!
|
||
7 | 1203 | deffi | #define vel_min 100 |
8 | 1159 | deffi | #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 | 1206 | deffi | |
28 | #define acceleration_increasing 1 |
||
29 | #define acceleration_decreasing 2 |
||
30 | |||
31 | static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t vel_start, uint8_t vel_end, int8_t vel_step) |
||
32 | 1159 | deffi | { |
33 | // Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4
|
||
34 | // which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is
|
||
35 | // easy and performance is not an issue here anyway.
|
||
36 | 1206 | deffi | |
37 | // Distinguish between start>=end/start<end because we have to use the corresponding stop condition.
|
||
38 | if (vel_end>=vel_start)
|
||
39 | for (uint16_t vel=vel_start; vel<=vel_end; vel+=vel_step)
|
||
40 | test_motors_direction_velocity (direction1, direction2, vel); |
||
41 | else
|
||
42 | for (uint16_t vel=vel_start; vel>=vel_end; vel+=vel_step)
|
||
43 | test_motors_direction_velocity (direction1, direction2, vel); |
||
44 | 1159 | deffi | |
45 | 1206 | deffi | |
46 | 1203 | deffi | // Send data
|
47 | for (uint8_t i=1; i<=2; ++i) |
||
48 | { |
||
49 | 1204 | deffi | uint8_t direction=(i==1)?direction1:direction2;
|
50 | |||
51 | 1205 | deffi | if (direction!=motor_direction_off)
|
52 | 1204 | deffi | { |
53 | usb_puts ("data motor ");
|
||
54 | usb_puti (i); |
||
55 | usb_puts (" ");
|
||
56 | 1205 | deffi | usb_puts (motor_direction_string (direction)); |
57 | 1206 | deffi | |
58 | if (vel_end>vel_start)
|
||
59 | usb_puts (" increasing ");
|
||
60 | else if (vel_end<vel_start) |
||
61 | usb_puts (" decreasing ");
|
||
62 | else
|
||
63 | usb_puts (" constant ");
|
||
64 | |||
65 | 1204 | deffi | usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL);
|
66 | } |
||
67 | 1203 | deffi | } |
68 | 1206 | deffi | } |
69 | 1203 | deffi | |
70 | 1206 | deffi | static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
71 | { |
||
72 | test_motors_direction_acceleration (direction1, direction2, vel_min, vel_max, vel_inc); |
||
73 | test_motors_direction_acceleration (direction1, direction2, vel_max, vel_min, -vel_inc); |
||
74 | 1159 | deffi | } |
75 | |||
76 | 1204 | deffi | |
77 | 1202 | deffi | void test_motor_all (void) |
78 | 1151 | deffi | { |
79 | 1166 | deffi | usb_puts("# Testing motors" NL);
|
80 | 1151 | deffi | |
81 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_backward); |
82 | test_motors_direction (motor_direction_backward, motor_direction_forward); |
||
83 | 1159 | deffi | |
84 | 1164 | deffi | robot_set_motors_off (); |
85 | 1159 | deffi | |
86 | 1166 | deffi | usb_puts("# Testing motors finished" NL);
|
87 | 1151 | deffi | } |
88 | 1202 | deffi | |
89 | void test_motor (uint8_t num)
|
||
90 | { |
||
91 | 1204 | deffi | if (num==1) |
92 | { |
||
93 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_off); |
94 | test_motors_direction (motor_direction_backward, motor_direction_off); |
||
95 | 1204 | deffi | } |
96 | else if (num==2) |
||
97 | { |
||
98 | 1205 | deffi | test_motors_direction (motor_direction_off, motor_direction_forward); |
99 | test_motors_direction (motor_direction_off, motor_direction_backward); |
||
100 | 1204 | deffi | } |
101 | 1202 | deffi | } |