Rev 1105 | Rev 1153 | Go to most recent revision | Show entire file | Regard 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; |
Line 191... | Line 194... | ||
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; |
|
Line -... | Line 220... | ||
- | 220 | ||
- | 221 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; // set on match |
|
- | 222 | //ServoValue -= OCR2A; |
|
197 | 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 | } |
|
- | 252 | } |