root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1215
History | View | Annotate | Download (3.04 KB)
1 | 1151 | deffi | #include "test_motors.h" |
---|---|---|---|
2 | #include "global.h" |
||
3 | |||
4 | 1182 | deffi | #include "comm_robot.h" |
5 | 1214 | deffi | #include "hardware_encoders.h" |
6 | 1159 | deffi | |
7 | 1215 | deffi | |
8 | // ******************
|
||
9 | // ** Data sending **
|
||
10 | // ******************
|
||
11 | |||
12 | 1214 | deffi | static void send_motor_data (uint8_t num, uint8_t direction, char *acceleration_string, |
13 | uint8_t num_steps, uint8_t *data_pwm, int16_t *data_velocity) |
||
14 | 1159 | deffi | { |
15 | 1214 | deffi | if (direction!=motor_direction_off)
|
16 | { |
||
17 | usb_puts ("data motor ");
|
||
18 | usb_puti (num); |
||
19 | usb_puts (" ");
|
||
20 | usb_puts (motor_direction_string (direction)); |
||
21 | usb_puts (" ");
|
||
22 | usb_puts (acceleration_string); |
||
23 | usb_puts (" ");
|
||
24 | |||
25 | for (uint8_t i=0; i<num_steps; ++i) |
||
26 | { |
||
27 | usb_putc (' ');
|
||
28 | usb_puti (data_pwm[i]); |
||
29 | usb_putc ('/');
|
||
30 | usb_puti (data_velocity[i]); |
||
31 | } |
||
32 | 1159 | deffi | |
33 | 1214 | deffi | usb_puts (NL); |
34 | } |
||
35 | 1159 | deffi | } |
36 | |||
37 | 1215 | deffi | |
38 | // ************************
|
||
39 | // ** Internal functions **
|
||
40 | // ************************
|
||
41 | |||
42 | 1214 | deffi | static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t pwm_start, uint8_t num_steps, int8_t pwm_step, char *acceleration_string) |
43 | { |
||
44 | // Allocate space for the data on the stack
|
||
45 | uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t ));
|
||
46 | int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t));
|
||
47 | int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t));
|
||
48 | 1206 | deffi | |
49 | 1214 | deffi | uint8_t pwm=pwm_start; |
50 | for (uint8_t i=0; i<num_steps; ++i) |
||
51 | { |
||
52 | robot_set_motors (direction1, pwm, direction2, pwm); |
||
53 | // TODO better
|
||
54 | #define vel_delay 400 |
||
55 | delay_ms (vel_delay); // TODO wait steady
|
||
56 | 1206 | deffi | |
57 | 1214 | deffi | data_pwm[i]=pwm; |
58 | 1206 | deffi | |
59 | 1214 | deffi | encoders_reset (); |
60 | // TODO delay
|
||
61 | encoders_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
||
62 | 1159 | deffi | |
63 | 1214 | deffi | pwm+=pwm_step; |
64 | 1203 | deffi | } |
65 | 1214 | deffi | |
66 | send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l);
|
||
67 | send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r);
|
||
68 | |||
69 | robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
||
70 | |||
71 | free (data_velocity_r); |
||
72 | free (data_velocity_l); |
||
73 | free (data_pwm); |
||
74 | 1206 | deffi | } |
75 | 1203 | deffi | |
76 | 1206 | deffi | static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
77 | { |
||
78 | 1214 | deffi | #define vel_inc 10 |
79 | #define vel_steps 10 |
||
80 | #define vel_min 140 |
||
81 | #define vel_max 250 |
||
82 | |||
83 | test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing");
|
||
84 | test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing");
|
||
85 | 1159 | deffi | } |
86 | |||
87 | 1204 | deffi | |
88 | 1215 | deffi | // **********************
|
89 | // ** Public functions **
|
||
90 | // **********************
|
||
91 | |||
92 | 1202 | deffi | void test_motor_all (void) |
93 | 1151 | deffi | { |
94 | 1166 | deffi | usb_puts("# Testing motors" NL);
|
95 | 1151 | deffi | |
96 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_backward); |
97 | test_motors_direction (motor_direction_backward, motor_direction_forward); |
||
98 | 1159 | deffi | |
99 | 1164 | deffi | robot_set_motors_off (); |
100 | 1159 | deffi | |
101 | 1166 | deffi | usb_puts("# Testing motors finished" NL);
|
102 | 1151 | deffi | } |
103 | 1202 | deffi | |
104 | void test_motor (uint8_t num)
|
||
105 | { |
||
106 | 1204 | deffi | if (num==1) |
107 | { |
||
108 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_off); |
109 | test_motors_direction (motor_direction_backward, motor_direction_off); |
||
110 | 1204 | deffi | } |
111 | else if (num==2) |
||
112 | { |
||
113 | 1205 | deffi | test_motors_direction (motor_direction_off, motor_direction_forward); |
114 | test_motors_direction (motor_direction_off, motor_direction_backward); |
||
115 | 1204 | deffi | } |
116 | 1202 | deffi | } |