root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1215
History | View | Annotate | Download (3.04 KB)
1 |
#include "test_motors.h" |
---|---|
2 |
#include "global.h" |
3 |
|
4 |
#include "comm_robot.h" |
5 |
#include "hardware_encoders.h" |
6 |
|
7 |
|
8 |
// ******************
|
9 |
// ** Data sending **
|
10 |
// ******************
|
11 |
|
12 |
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 |
{ |
15 |
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 |
|
33 |
usb_puts (NL); |
34 |
} |
35 |
} |
36 |
|
37 |
|
38 |
// ************************
|
39 |
// ** Internal functions **
|
40 |
// ************************
|
41 |
|
42 |
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 |
|
49 |
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 |
|
57 |
data_pwm[i]=pwm; |
58 |
|
59 |
encoders_reset (); |
60 |
// TODO delay
|
61 |
encoders_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
62 |
|
63 |
pwm+=pwm_step; |
64 |
} |
65 |
|
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 |
} |
75 |
|
76 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
77 |
{ |
78 |
#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 |
} |
86 |
|
87 |
|
88 |
// **********************
|
89 |
// ** Public functions **
|
90 |
// **********************
|
91 |
|
92 |
void test_motor_all (void) |
93 |
{ |
94 |
usb_puts("# Testing motors" NL);
|
95 |
|
96 |
test_motors_direction (motor_direction_forward, motor_direction_backward); |
97 |
test_motors_direction (motor_direction_backward, motor_direction_forward); |
98 |
|
99 |
robot_set_motors_off (); |
100 |
|
101 |
usb_puts("# Testing motors finished" NL);
|
102 |
} |
103 |
|
104 |
void test_motor (uint8_t num)
|
105 |
{ |
106 |
if (num==1) |
107 |
{ |
108 |
test_motors_direction (motor_direction_forward, motor_direction_off); |
109 |
test_motors_direction (motor_direction_backward, motor_direction_off); |
110 |
} |
111 |
else if (num==2) |
112 |
{ |
113 |
test_motors_direction (motor_direction_off, motor_direction_forward); |
114 |
test_motors_direction (motor_direction_off, motor_direction_backward); |
115 |
} |
116 |
} |