root / trunk / code / projects / diagnostic_station / station / hardware_turntable.c @ 1390
History | View | Annotate | Download (3.1 KB)
1 | 1183 | deffi | #include "hardware_turntable.h" |
---|---|---|---|
2 | 1255 | bneuman | #include <encoders.h> |
3 | 1183 | deffi | |
4 | 1307 | bneuman | #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 | 1183 | deffi | void turntable_init ()
|
24 | { |
||
25 | 1262 | bneuman | int e;
|
26 | 1255 | bneuman | encoders_init(); |
27 | 1262 | bneuman | |
28 | 1307 | bneuman | encoder_wait(1);
|
29 | |||
30 | 1262 | bneuman | // get a reading to check alignment
|
31 | 1307 | bneuman | e = encoder_read(TURNTABLE_ENCODER); |
32 | 1262 | bneuman | if(e==ENCODER_MAGNET_FAILURE)
|
33 | 1289 | deffi | usb_puts("# ERROR: turntable encoder magnet failure!!" NL);
|
34 | 1262 | bneuman | else if(e==ENCODER_MISALIGNED) |
35 | 1289 | deffi | usb_puts("# ERROR: turntable encoder misaligned!!" NL);
|
36 | 1262 | bneuman | else if(e==ENCODER_DATA_NOT_READY) |
37 | 1289 | deffi | usb_puts("# ERROR: turntable encoder data not ready!!" NL);
|
38 | 1283 | bneuman | |
39 | 1307 | bneuman | //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 | 1283 | bneuman | motors_init(); |
49 | 1183 | deffi | } |
50 | |||
51 | 1255 | bneuman | void turntable_rotate_to_position (int16_t position)
|
52 | 1183 | deffi | { |
53 | 1307 | bneuman | int16_t turntable=0;
|
54 | uint8_t direction=0, last_direction=0; |
||
55 | int16_t error = 0;
|
||
56 | 1283 | bneuman | |
57 | 1307 | bneuman | //must be 16 bit to check for overflow
|
58 | uint16_t power=0;
|
||
59 | uint8_t cycle_count=0;
|
||
60 | 1287 | bneuman | |
61 | 1255 | bneuman | usb_puts ("# Turntable rotating to ");
|
62 | usb_puti (position); |
||
63 | usb_puts (NL); |
||
64 | 1283 | bneuman | |
65 | 1307 | bneuman | /* 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 | 1283 | bneuman | { |
73 | 1307 | bneuman | |
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 | 1283 | bneuman | |
80 | 1307 | bneuman | //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 | 1283 | bneuman | } |
118 | 1307 | bneuman | |
119 | encoder_wait(TURNTABLE_STABLE_READINGS); |
||
120 | 1283 | bneuman | } |
121 | |||
122 | 1307 | bneuman | |
123 | MOTOR_TURNTABLE_SET(direction, 0);
|
||
124 | |||
125 | //just to prevent resetting
|
||
126 | delay_ms(50);
|
||
127 | 1183 | deffi | |
128 | 1255 | bneuman | usb_puts ("# Turntable position reached" NL);
|
129 | 1183 | deffi | } |
130 | |||
131 | 1244 | deffi | uint16_t turntable_get_position (void)
|
132 | { |
||
133 | 1307 | bneuman | return encoder_get_dx(TURNTABLE_ENCODER) + turntable_encoder_offset;
|
134 | // return encoder_read(TURNTABLE_ENCODER);
|
||
135 | 1244 | deffi | } |