root / trunk / code / projects / diagnostic_station / station / hardware_turntable.c @ 1283
History | View | Annotate | Download (1.2 KB)
1 |
#include "hardware_turntable.h" |
---|---|
2 |
#include <encoders.h> |
3 |
|
4 |
void turntable_init ()
|
5 |
{ |
6 |
int e;
|
7 |
encoders_init(); |
8 |
|
9 |
// get a reading to check alignment
|
10 |
e = encoder_read(LEFT); |
11 |
if(e==ENCODER_MAGNET_FAILURE)
|
12 |
usb_puts("# ERROR: turntable encoder magnet failure!!");
|
13 |
else if(e==ENCODER_MISALIGNED) |
14 |
usb_puts("# ERROR: turntable encoder misaligned!!");
|
15 |
else if(e==ENCODER_DATA_NOT_READY) |
16 |
usb_puts("# ERROR: turntable encoder data not ready!!");
|
17 |
else
|
18 |
usb_puts("# Turntable encoder initialized");
|
19 |
|
20 |
motors_init(); |
21 |
} |
22 |
|
23 |
void turntable_rotate_to_position (int16_t position)
|
24 |
{ |
25 |
uint16_t turntable; |
26 |
|
27 |
usb_puts ("# Turntable rotating to ");
|
28 |
usb_puti (position); |
29 |
usb_puts (NL); |
30 |
|
31 |
//TODO: p control
|
32 |
//TODO: proper direction
|
33 |
motor1_set(1, 180); |
34 |
|
35 |
while((turntable = turntable_get_position()) <= position)
|
36 |
{ |
37 |
delay_ms(1);
|
38 |
usb_puti(turntable); |
39 |
usb_puts(NL); |
40 |
|
41 |
if(turntable >= 1024) |
42 |
{ |
43 |
usb_puts("# ERROR: magnetic encoder error!" NL);
|
44 |
break;
|
45 |
} |
46 |
} |
47 |
|
48 |
motor1_set(1, 0); |
49 |
|
50 |
usb_puts ("# Turntable position reached" NL);
|
51 |
} |
52 |
|
53 |
uint16_t turntable_get_position (void)
|
54 |
{ |
55 |
//TODO: convert units
|
56 |
return encoder_read(RIGHT);
|
57 |
} |