Rev 1105 | Rev 1153 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1105 | Rev 1111 | ||
---|---|---|---|
Line 155... | Line 155... | ||
155 | 155 | ||
156 | SIGNAL(SIG_OUTPUT_COMPARE2A) |
156 | SIGNAL(SIG_OUTPUT_COMPARE2A) |
157 | { |
157 | { |
158 | static unsigned char postPulse = 0x80; |
158 | static unsigned char postPulse = 0x80; |
- | 159 | static int filterServo = 100; |
|
159 | static int filterServo = 100; |
160 | static unsigned char restPulse = 50; |
- | 161 | #define MULTIPLIER 4 |
|
- | 162 | if(PlatinenVersion < 20) |
|
160 | #define MULTIPLIER 4 |
163 | { |
161 | if(ServoState == 4) |
164 | if(ServoState == 4) |
162 | { |
165 | { |
163 | ServoValue = 0x0030; // Offset Part1 |
166 | ServoValue = 0x0030; // Offset Part1 |
164 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
167 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
165 | ServoValue += filterServo; |
168 | ServoValue += filterServo; |
Line 171... | Line 174... | ||
171 | DebugOut.Analog[20] = ServoValue; |
174 | DebugOut.Analog[20] = ServoValue; |
172 | if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
175 | if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
173 | OCR2A = 255-(ServoValue % 256); |
176 | OCR2A = 255-(ServoValue % 256); |
174 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
177 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
175 | } |
178 | } |
176 | else if ((ServoState > 0) && (ServoState < 4)) |
179 | else if ((ServoState > 0) && (ServoState < 4)) |
177 | { |
180 | { |
178 | if(ServoValue > 255) |
181 | if(ServoValue > 255) |
179 | { PORTD |= 0x80; |
182 | { PORTD |= 0x80; |
180 | TCCR2A =3; |
183 | TCCR2A =3; |
181 | ServoValue -= 255; |
184 | ServoValue -= 255; |
Line 184... | Line 187... | ||
184 | { |
187 | { |
185 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3; |
188 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3; |
186 | OCR2A = postPulse; // Offset Part2 |
189 | OCR2A = postPulse; // Offset Part2 |
187 | ServoState = 1; |
190 | ServoState = 1; |
188 | } |
191 | } |
189 | } |
192 | } |
190 | else if (ServoState == 0) |
193 | else if (ServoState == 0) |
191 | { |
194 | { |
192 | ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER; |
195 | ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER; |
193 | PORTD&=~0x80; |
196 | PORTD&=~0x80; |
194 | TCCR2A = 3; |
197 | TCCR2A = 3; |
195 | } |
198 | } |
- | 199 | } |
|
- | 200 | else |
|
- | 201 | { |
|
196 | ServoState--; |
202 | if(ServoState == 4) |
- | 203 | { |
|
- | 204 | PORTD &= ~0x80; |
|
- | 205 | PORTC |= _BV(PC6); |
|
- | 206 | ServoValue = 0x00030; // Offset Part1 |
|
- | 207 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
|
- | 208 | ServoValue += filterServo; |
|
- | 209 | if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) ); |
|
- | 210 | else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) ); |
|
- | 211 | if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
|
- | 212 | else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
|
- | 213 | PORTD |= 0x80; // dummy clock to skip output Q0 |
|
- | 214 | if ((ServoValue % 256) < 1) ServoValue -=2; |
|
- | 215 | if ((ServoValue % 256) > 253) ServoValue +=2; |
|
- | 216 | DebugOut.Analog[20] = ServoValue; |
|
- | 217 | ||
- | 218 | OCR2A = 254-(ServoValue % 255); |
|
- | 219 | PORTD &= ~0x80; |
|
197 | 220 | ||
- | 221 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; // set on match |
|
- | 222 | //ServoValue -= OCR2A; |
|
- | 223 | PORTC &= ~_BV(PC6); |
|
- | 224 | } |
|
- | 225 | else if ((ServoState > 0) && (ServoState < 4)) |
|
- | 226 | { |
|
- | 227 | if(ServoValue > 255) |
|
- | 228 | { |
|
- | 229 | PORTD &= ~0x80; |
|
- | 230 | TCCR2A =3; |
|
- | 231 | ServoValue -= 255; |
|
- | 232 | OCR2A = postPulse; // Offset Part2 |
|
- | 233 | //OCR2A = (ServoValue % 256); |
|
- | 234 | } |
|
- | 235 | else |
|
- | 236 | { |
|
- | 237 | OCR2A = postPulse; // Offset Part2 |
|
- | 238 | //OCR2A = (ServoValue % 256); |
|
- | 239 | //TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
|
- | 240 | ServoState = 1; |
|
- | 241 | } |
|
- | 242 | } |
|
- | 243 | else if (ServoState == 0) |
|
- | 244 | { |
|
- | 245 | PORTD &= ~0x80; |
|
- | 246 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
|
- | 247 | ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER; |
|
- | 248 | TCCR2A =3; |
|
- | 249 | } |
|
- | 250 | ServoState--; |
|
- | 251 | } |
|
198 | } |
252 | } |
- | 253 |