root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1439
History | View | Annotate | Download (4.79 KB)
1 | 1151 | deffi | #include "test_motors.h" |
---|---|---|---|
2 | #include "global.h" |
||
3 | |||
4 | 1182 | deffi | #include "comm_robot.h" |
5 | 1257 | bneuman | #include "hardware_dynamos.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 | 1305 | deffi | // One way to do it
|
43 | 1318 | bneuman | /* #define vel_inc 10 */
|
44 | /* #define vel_steps 12 */
|
||
45 | /* #define vel_min 140 */
|
||
46 | /* #define vel_max 250 */
|
||
47 | /* #define velocity_steady_delay 400 */
|
||
48 | |||
49 | // More steps at smaller intervals. We don't need so much delay here.
|
||
50 | #define vel_inc 2 |
||
51 | #define vel_steps 56 |
||
52 | #define vel_min 140 |
||
53 | #define vel_max 250 |
||
54 | #define velocity_steady_delay 50 |
||
55 | |||
56 | // #define vel_inc 2
|
||
57 | 1305 | deffi | // #define vel_steps 12
|
58 | 1318 | bneuman | // #define vel_min 150
|
59 | // #define vel_max 172
|
||
60 | // #define velocity_steady_delay 50
|
||
61 | 1250 | deffi | |
62 | 1305 | deffi | |
63 | |||
64 | 1318 | bneuman | |
65 | 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) |
66 | { |
||
67 | // Allocate space for the data on the stack
|
||
68 | 1318 | bneuman | // uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t ));
|
69 | // int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t));
|
||
70 | // int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t));
|
||
71 | 1206 | deffi | |
72 | 1214 | deffi | uint8_t pwm=pwm_start; |
73 | 1318 | bneuman | int16_t left; |
74 | int16_t right; |
||
75 | |||
76 | 1214 | deffi | for (uint8_t i=0; i<num_steps; ++i) |
77 | { |
||
78 | robot_set_motors (direction1, pwm, direction2, pwm); |
||
79 | 1206 | deffi | |
80 | 1250 | deffi | // Wait some fixed time for the velocity to reach steady state. This could be improved.
|
81 | delay_ms (velocity_steady_delay); |
||
82 | |||
83 | 1318 | bneuman | // Store it
|
84 | //data_pwm[i]=pwm;
|
||
85 | 1206 | deffi | |
86 | 1251 | deffi | // Reset the dynamos, wait some time, then read the dynamos.
|
87 | // Nothing else should go between dynamos_reset and dynamos_read.
|
||
88 | 1250 | deffi | // Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.
|
89 | 1251 | deffi | dynamos_reset (); |
90 | 1250 | deffi | delay_ms (500);
|
91 | 1318 | bneuman | dynamos_read (&left, &right); |
92 | |||
93 | // Velocity readings are dynamo ticks per second.
|
||
94 | left*=2;
|
||
95 | right*=2;
|
||
96 | |||
97 | // Store it
|
||
98 | //dynamos_read (&(data_velocity_l[i]), &(data_velocity_r[i]));
|
||
99 | 1250 | deffi | |
100 | 1318 | bneuman | |
101 | // Bad things happening. Methinks it's a stack overflow. Print one data line
|
||
102 | // per measurement. The plotting script can probably handle it. Not sure about
|
||
103 | // the server.
|
||
104 | send_motor_data (0, direction1, acceleration_string, 1, &pwm, &left); |
||
105 | send_motor_data (1, direction2, acceleration_string, 1, &pwm, &right); |
||
106 | |||
107 | //usb_puts ("# ");
|
||
108 | //usb_puti (i);
|
||
109 | //usb_puts (" ");
|
||
110 | //usb_puti (pwm);
|
||
111 | //usb_puts (" ");
|
||
112 | //usb_puti (data_velocity_l[i]);
|
||
113 | //usb_puts (" ");
|
||
114 | //usb_puti (data_velocity_r[i]);
|
||
115 | //usb_puts (NL);
|
||
116 | |||
117 | 1214 | deffi | pwm+=pwm_step; |
118 | 1203 | deffi | } |
119 | 1214 | deffi | |
120 | 1318 | bneuman | // send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l);
|
121 | // send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r);
|
||
122 | 1214 | deffi | |
123 | robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
||
124 | |||
125 | 1318 | bneuman | // free (data_velocity_r);
|
126 | // free (data_velocity_l);
|
||
127 | // free (data_pwm);
|
||
128 | 1206 | deffi | } |
129 | 1203 | deffi | |
130 | 1206 | deffi | static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
131 | { |
||
132 | 1250 | deffi | // The following equation must be true: vel_min+(vel_steps-1)*vel_inc=vel_max
|
133 | // Note that we're setting vel_max twice, once going up and once going down. It's not important for vel_max, but
|
||
134 | // vel_min is also reached twice, once at the beginning of accelerating and once at the end of decelerating, and
|
||
135 | // this one is important.
|
||
136 | |||
137 | 1214 | deffi | test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing");
|
138 | test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing");
|
||
139 | 1159 | deffi | } |
140 | |||
141 | 1204 | deffi | |
142 | 1215 | deffi | // **********************
|
143 | // ** Public functions **
|
||
144 | // **********************
|
||
145 | |||
146 | 1202 | deffi | void test_motor_all (void) |
147 | 1151 | deffi | { |
148 | 1166 | deffi | usb_puts("# Testing motors" NL);
|
149 | 1151 | deffi | |
150 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_backward); |
151 | test_motors_direction (motor_direction_backward, motor_direction_forward); |
||
152 | 1159 | deffi | |
153 | 1164 | deffi | robot_set_motors_off (); |
154 | 1159 | deffi | |
155 | 1166 | deffi | usb_puts("# Testing motors finished" NL);
|
156 | 1151 | deffi | } |
157 | 1202 | deffi | |
158 | void test_motor (uint8_t num)
|
||
159 | { |
||
160 | 1204 | deffi | if (num==1) |
161 | { |
||
162 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_off); |
163 | test_motors_direction (motor_direction_backward, motor_direction_off); |
||
164 | 1204 | deffi | } |
165 | else if (num==2) |
||
166 | { |
||
167 | 1205 | deffi | test_motors_direction (motor_direction_off, motor_direction_forward); |
168 | test_motors_direction (motor_direction_off, motor_direction_backward); |
||
169 | 1204 | deffi | } |
170 | 1202 | deffi | } |