Revision 1307
Added turntable control code. Currently uses PI control and is having lots of mechanical issues
Also added a main_brad which moves the turntable back and forth a bit
added a return to home value for the turntable
trunk/code/projects/diagnostic_station/station/hardware_turntable.c | ||
---|---|---|
1 | 1 |
#include "hardware_turntable.h" |
2 | 2 |
#include <encoders.h> |
3 | 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 |
|
|
4 | 23 |
void turntable_init () |
5 | 24 |
{ |
6 | 25 |
int e; |
7 | 26 |
encoders_init(); |
8 | 27 |
|
28 |
encoder_wait(1); |
|
29 |
|
|
9 | 30 |
// get a reading to check alignment |
10 |
e = encoder_read(LEFT);
|
|
31 |
e = encoder_read(TURNTABLE_ENCODER);
|
|
11 | 32 |
if(e==ENCODER_MAGNET_FAILURE) |
12 | 33 |
usb_puts("# ERROR: turntable encoder magnet failure!!" NL); |
13 | 34 |
else if(e==ENCODER_MISALIGNED) |
14 | 35 |
usb_puts("# ERROR: turntable encoder misaligned!!" NL); |
15 | 36 |
else if(e==ENCODER_DATA_NOT_READY) |
16 | 37 |
usb_puts("# ERROR: turntable encoder data not ready!!" NL); |
17 |
else |
|
18 |
usb_puts("# Turntable encoder initialized" NL); |
|
19 | 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 |
|
|
20 | 48 |
motors_init(); |
21 | 49 |
} |
22 | 50 |
|
23 | 51 |
void turntable_rotate_to_position (int16_t position) |
24 | 52 |
{ |
25 |
uint16_t turntable; |
|
53 |
int16_t turntable=0; |
|
54 |
uint8_t direction=0, last_direction=0; |
|
55 |
int16_t error = 0; |
|
26 | 56 |
|
27 |
return; |
|
57 |
//must be 16 bit to check for overflow |
|
58 |
uint16_t power=0; |
|
59 |
uint8_t cycle_count=0; |
|
28 | 60 |
|
29 | 61 |
usb_puts ("# Turntable rotating to "); |
30 | 62 |
usb_puti (position); |
31 | 63 |
usb_puts (NL); |
32 |
|
|
33 |
//TODO: p control |
|
34 |
//TODO: proper direction |
|
35 |
motor1_set(1, 180); |
|
36 | 64 |
|
37 |
while((turntable = turntable_get_position()) <= position) |
|
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) |
|
38 | 72 |
{ |
39 |
delay_ms(1); |
|
40 |
usb_puti(turntable); |
|
41 |
usb_puts(NL); |
|
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; |
|
42 | 79 |
|
43 |
if(turntable >= 1024) |
|
44 |
{ |
|
45 |
usb_puts("# ERROR: magnetic encoder error!" NL); |
|
46 |
break; |
|
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); |
|
47 | 117 |
} |
118 |
|
|
119 |
encoder_wait(TURNTABLE_STABLE_READINGS); |
|
48 | 120 |
} |
49 | 121 |
|
50 |
motor1_set(1, 0); |
|
122 |
|
|
123 |
MOTOR_TURNTABLE_SET(direction, 0); |
|
124 |
|
|
125 |
//just to prevent resetting |
|
126 |
delay_ms(50); |
|
51 | 127 |
|
52 | 128 |
usb_puts ("# Turntable position reached" NL); |
53 | 129 |
} |
54 | 130 |
|
55 | 131 |
uint16_t turntable_get_position (void) |
56 | 132 |
{ |
57 |
//TODO: convert units
|
|
58 |
return encoder_read(RIGHT);
|
|
133 |
return encoder_get_dx(TURNTABLE_ENCODER) + turntable_encoder_offset;
|
|
134 |
// return encoder_read(TURNTABLE_ENCODER);
|
|
59 | 135 |
} |
Also available in: Unified diff