root / trunk / code / projects / diagnostic_station / station / hardware_turntable.c @ 1307
History | View | Annotate | Download (3.1 KB)
1 |
#include "hardware_turntable.h" |
---|---|
2 |
#include <encoders.h> |
3 |
|
4 |
#define TURNTABLE_ENCODER_ZERO 737 |
5 |
|
6 |
#define MOTOR_TURNTABLE_SET motor1_set
|
7 |
#define TURNTABLE_ENCODER RIGHT
|
8 |
#define TURNTABLE_MAX_PWM 255 |
9 |
|
10 |
#define TURNTABLE_TOLERANCE 5 |
11 |
#define TURNTABLE_STABLE_READINGS 100 |
12 |
|
13 |
//180 seems to be just below
|
14 |
#define TURNTABLE_PWM_OFFSET 160 |
15 |
#define TURNTABLE_P 2 |
16 |
|
17 |
//this seems like it should be between 50:200
|
18 |
//100 is a little slow, but probably good
|
19 |
#define TURNTABLE_I_DIV 100 |
20 |
|
21 |
int16_t turntable_encoder_offset = 0;
|
22 |
|
23 |
void turntable_init ()
|
24 |
{ |
25 |
int e;
|
26 |
encoders_init(); |
27 |
|
28 |
encoder_wait(1);
|
29 |
|
30 |
// get a reading to check alignment
|
31 |
e = encoder_read(TURNTABLE_ENCODER); |
32 |
if(e==ENCODER_MAGNET_FAILURE)
|
33 |
usb_puts("# ERROR: turntable encoder magnet failure!!" NL);
|
34 |
else if(e==ENCODER_MISALIGNED) |
35 |
usb_puts("# ERROR: turntable encoder misaligned!!" NL);
|
36 |
else if(e==ENCODER_DATA_NOT_READY) |
37 |
usb_puts("# ERROR: turntable encoder data not ready!!" NL);
|
38 |
|
39 |
//get the offset
|
40 |
turntable_encoder_offset = e - TURNTABLE_ENCODER_ZERO; |
41 |
//make it be less than 512
|
42 |
while(turntable_encoder_offset > 512) |
43 |
turntable_encoder_offset -= 1024;
|
44 |
while(turntable_encoder_offset < -512) |
45 |
turntable_encoder_offset += 1024;
|
46 |
|
47 |
|
48 |
motors_init(); |
49 |
} |
50 |
|
51 |
void turntable_rotate_to_position (int16_t position)
|
52 |
{ |
53 |
int16_t turntable=0;
|
54 |
uint8_t direction=0, last_direction=0; |
55 |
int16_t error = 0;
|
56 |
|
57 |
//must be 16 bit to check for overflow
|
58 |
uint16_t power=0;
|
59 |
uint8_t cycle_count=0;
|
60 |
|
61 |
usb_puts ("# Turntable rotating to ");
|
62 |
usb_puti (position); |
63 |
usb_puts (NL); |
64 |
|
65 |
/* MOTOR_TURNTABLE_SET(1, TURNTABLE_PWM_OFFSET);
|
66 |
|
67 |
delay_ms(1000);
|
68 |
return;
|
69 |
*/
|
70 |
|
71 |
while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE)
|
72 |
{ |
73 |
|
74 |
while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE)
|
75 |
{ |
76 |
//figure out which direction to set the motor
|
77 |
last_direction = direction; |
78 |
direction = turntable < position; |
79 |
|
80 |
//clear the accumuliated error when switching directions
|
81 |
if(last_direction != direction)
|
82 |
error = 0;
|
83 |
|
84 |
|
85 |
//only add to the error every TURNTABLE_I_DIV cycles
|
86 |
if(++cycle_count > TURNTABLE_I_DIV)
|
87 |
{ |
88 |
error += turntable - position; |
89 |
cycle_count = 0;
|
90 |
} |
91 |
|
92 |
power = abs(TURNTABLE_P*(turntable - position) + error) + TURNTABLE_PWM_OFFSET; |
93 |
if(power > TURNTABLE_MAX_PWM)
|
94 |
power = TURNTABLE_MAX_PWM; |
95 |
MOTOR_TURNTABLE_SET(direction, power); |
96 |
|
97 |
|
98 |
/* usb_puti(turntable); */
|
99 |
/* usb_puts(NL); */
|
100 |
|
101 |
if(turntable >= 1024) |
102 |
{ |
103 |
usb_puts("# ERROR: magnetic encoder error!" NL);
|
104 |
break;
|
105 |
} |
106 |
|
107 |
usb_puti(error); |
108 |
usb_puts(" ");
|
109 |
usb_puti(direction); |
110 |
usb_puts(" ");
|
111 |
usb_puti(power); |
112 |
usb_puts(NL); |
113 |
|
114 |
delay_ms(1);
|
115 |
|
116 |
encoder_wait(1);
|
117 |
} |
118 |
|
119 |
encoder_wait(TURNTABLE_STABLE_READINGS); |
120 |
} |
121 |
|
122 |
|
123 |
MOTOR_TURNTABLE_SET(direction, 0);
|
124 |
|
125 |
//just to prevent resetting
|
126 |
delay_ms(50);
|
127 |
|
128 |
usb_puts ("# Turntable position reached" NL);
|
129 |
} |
130 |
|
131 |
uint16_t turntable_get_position (void)
|
132 |
{ |
133 |
return encoder_get_dx(TURNTABLE_ENCODER) + turntable_encoder_offset;
|
134 |
// return encoder_read(TURNTABLE_ENCODER);
|
135 |
} |