Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1915 | - | 1 | |
2 | /********************************************************************/ |
||
3 | /* */ |
||
4 | /* NG-Video 5,8GHz */ |
||
5 | /* */ |
||
6 | /* Copyright (C) 2011 - gebad */ |
||
7 | /* */ |
||
8 | /* This code is distributed under the GNU Public License */ |
||
9 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
10 | /* */ |
||
11 | /* using */ |
||
12 | /*! \file servo.c \brief Interrupt-driven RC Servo function library.*/ |
||
13 | /* */ |
||
14 | /*File Name : 'servo.c' */ |
||
15 | /*Title : Interrupt-driven RC Servo function library */ |
||
16 | /*Author : Pascal Stang - Copyright (C) 2002 */ |
||
17 | /*Created : 7/31/2002 */ |
||
18 | /*Revised : 8/02/2002 */ |
||
19 | /*Version : 1.0 */ |
||
20 | /*Target MCU : Atmel AVR Series */ |
||
21 | /*Editor Tabs : 2 */ |
||
22 | /* */ |
||
23 | /*This code is distributed under the GNU Public License */ |
||
24 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
25 | /* */ |
||
26 | /********************************************************************/ |
||
27 | |||
28 | #include <avr/interrupt.h> |
||
29 | #include <avr/io.h> |
||
30 | |||
31 | #include "../tracking/ng_servo.h" |
||
32 | #include "../tracking/tracking.h" |
||
33 | #include "../HAL_HW3_9.h" |
||
34 | #include "../eeprom/eeprom.h" |
||
35 | //#include "config.h" |
||
36 | |||
37 | // Global variables |
||
38 | uint16_t ServoPosTics; |
||
39 | uint16_t ServoPeriodTics; |
||
40 | |||
41 | // servo channel registers |
||
42 | ServoChannelType ServoChannels[SERVO_NUM_CHANNELS]; |
||
43 | |||
44 | const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER}, |
||
45 | {SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}}; |
||
46 | |||
47 | // Servo limits (depend on hardware) |
||
48 | const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX}, |
||
49 | {SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX}, |
||
50 | {SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}}, |
||
51 | {{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX}, |
||
52 | {SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX}, |
||
53 | {SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}}; |
||
54 | |||
55 | // Servopositionen normiert für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1 |
||
56 | const steps_pw_t steps_pw[2] = {{(uint64_t)950*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*F_CPU*1e-6/SERVO_PRESCALER + 0.5,(uint64_t)1500*F_CPU*1e-6/SERVO_PRESCALER + 0.5}, |
||
57 | {(uint64_t)950*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5}}; |
||
58 | |||
59 | // anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX |
||
60 | const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 }; |
||
61 | |||
62 | // functions |
||
63 | void servo_test(void) |
||
64 | { |
||
65 | //Dummy |
||
66 | } |
||
67 | //! initializes software PWM system 16-bit Timer |
||
68 | // normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt |
||
69 | // Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!! |
||
70 | void servoInit(uint8_t servo_period) |
||
71 | { uint16_t OCValue; // set initial interrupt time |
||
72 | |||
73 | |||
74 | |||
75 | |||
76 | // disble Timer/Counter1, Output Compare A Match Interrupt |
||
77 | TIMSK1 &= ~(1<<OCIE1A); |
||
78 | // set the prescaler for timer1 |
||
79 | if (Config.sIdxSteps == STEPS_255) { |
||
80 | TCCR1B &= ~((1<<CS11) | (1<<CS10)); |
||
81 | TCCR1B |= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel |
||
82 | } |
||
83 | else { |
||
84 | TCCR1B &= ~(1<<CS12); |
||
85 | TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel |
||
86 | } |
||
87 | // attach the software PWM service routine to timer1 output compare A |
||
88 | // timerAttach(TIMER1OUTCOMPAREA_INT, servoService); |
||
89 | // enable channels |
||
90 | for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) { |
||
91 | // set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5 |
||
92 | ServoChannels[channel].pin = (1 << (SERVO2 + channel)); |
||
93 | } |
||
94 | ServoPosTics = 0; // set PosTics |
||
95 | // set PeriodTics |
||
96 | ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3; |
||
97 | // read in current value of output compare register OCR1A |
||
98 | OCValue = OCR1AL; // read low byte of OCR1A |
||
99 | OCValue += (OCR1AH << 8); // read high byte of OCR1A |
||
100 | OCValue += ServoPeriodTics; // increment OCR1A value by nextTics |
||
101 | // set future output compare time to this new value |
||
102 | OCR1AH = OCValue >> 8; // write high byte |
||
103 | OCR1AL = OCValue & 0x00FF; // write low byte |
||
104 | TIMSK1 |= (1<<OCIE1A); // enable the timer1 output compare A interrupt |
||
105 | coldstart = 1; |
||
106 | } |
||
107 | |||
108 | void servoSetDefaultPos(void) |
||
109 | { |
||
110 | servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN |
||
111 | servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT |
||
112 | } |
||
113 | |||
114 | uint16_t pw_us(uint16_t Steps) |
||
115 | { |
||
116 | return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in µs |
||
117 | } |
||
118 | |||
119 | uint16_t ServoSteps(void) |
||
120 | { |
||
121 | return(ServoConst[Config.sIdxSteps].steps); |
||
122 | } |
||
123 | |||
124 | void servoSet_rev(uint8_t channel, uint8_t val) |
||
125 | { |
||
126 | ServoChannels[channel].rev = val & 0x01; |
||
127 | } |
||
128 | |||
129 | void servoSet_min(uint8_t channel, uint16_t min) |
||
130 | { |
||
131 | ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min; |
||
132 | } |
||
133 | |||
134 | void servoSet_max(uint8_t channel, uint16_t max) |
||
135 | { |
||
136 | ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max; |
||
137 | } |
||
138 | |||
139 | void servoSet_mid(uint8_t channel, uint16_t mid) |
||
140 | { |
||
141 | ServoChannels[channel].mid = mid; |
||
142 | // Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen |
||
143 | ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \ |
||
144 | (16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min; |
||
145 | } |
||
146 | |||
147 | //! turns off software PWM system |
||
148 | void servoOff(void) |
||
149 | { |
||
150 | // disable the timer1 output compare A interrupt |
||
151 | TIMSK1 &= ~(1<<OCIE1A); |
||
152 | } |
||
153 | |||
154 | //! set servo position on channel (raw unscaled format) |
||
155 | void servoSetPositionRaw(uint8_t channel, uint16_t position) |
||
156 | { |
||
157 | // bind to limits |
||
158 | if (position < ServoChannels[channel].min) position = ServoChannels[channel].min; |
||
159 | if (position > ServoChannels[channel].max) position = ServoChannels[channel].max; |
||
160 | // set position |
||
161 | ServoChannels[channel].duty = position; |
||
162 | } |
||
163 | |||
164 | //! set servo position on channel |
||
165 | // input should be between 0 and ServoSteps() (entspricht Maximalausschlag) |
||
166 | void servoSetPosition(uint8_t channel, uint16_t position) |
||
167 | { uint16_t pos_scaled; |
||
168 | |||
169 | // calculate scaled position |
||
170 | if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position; |
||
171 | if (position < ServoConst[Config.sIdxSteps].steps/2) |
||
172 | //bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2) |
||
173 | pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \ |
||
174 | ServoConst[Config.sIdxSteps].steps; |
||
175 | else |
||
176 | //bei Position >= Servomittelstellung |
||
177 | pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \ |
||
178 | * 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps; |
||
179 | // set position |
||
180 | servoSetPositionRaw(channel, pos_scaled); |
||
181 | } |
||
182 | |||
183 | // Umrechnung Winkel in Servoschritte |
||
184 | void servoSetAngle(uint8_t servo_nr, int16_t angle) |
||
185 | { |
||
186 | servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180)); |
||
187 | } |
||
188 | |||
189 | //Interruptroutine |
||
190 | ISR(TIMER1_COMPA_vect) |
||
191 | { static uint8_t ServoChannel; |
||
192 | uint16_t nextTics; |
||
193 | uint16_t OCValue; // schedule next interrupt |
||
194 | |||
195 | if(ServoChannel < SERVO_NUM_CHANNELS) { |
||
196 | PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel |
||
197 | } |
||
198 | ServoChannel++; // next channel |
||
199 | if(ServoChannel != SERVO_NUM_CHANNELS) { |
||
200 | // loop to channel 0 if needed |
||
201 | if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
||
202 | // turn on new channel |
||
203 | PORTD |= ServoChannels[ServoChannel].pin; |
||
204 | // schedule turn off time |
||
205 | nextTics = ServoChannels[ServoChannel].duty; |
||
206 | } |
||
207 | else { |
||
208 | // ***we could save time by precalculating this |
||
209 | // schedule end-of-period |
||
210 | nextTics = ServoPeriodTics-ServoPosTics; |
||
211 | } |
||
212 | // read in current value of output compare register OCR1A |
||
213 | OCValue = OCR1AL; // read low byte of OCR1A |
||
214 | OCValue += (OCR1AH <<8); // read high byte of OCR1A |
||
215 | OCValue += nextTics; // increment OCR1A value by nextTics |
||
216 | // set future output compare time to this new value |
||
217 | OCR1AH = OCValue >> 8; // write high byte |
||
218 | OCR1AL = OCValue & 0x00FF; // write low byte |
||
219 | |||
220 | ServoPosTics += nextTics; // set our new tic position |
||
221 | if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
||
222 | } |
||
223 |