Rev 1022 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1022 | Rev 1023 | ||
|---|---|---|---|
| Line 161... | Line 161... | ||
| 161 | if(ServoState == 4) |
161 | if(ServoState == 4) |
| 162 | { |
162 | { |
| 163 | ServoValue = 0x0030; // Offset Part1 |
163 | ServoValue = 0x0030; // Offset Part1 |
| 164 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
164 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
| 165 | ServoValue += filterServo; |
165 | ServoValue += filterServo; |
| - | 166 | ||
| 166 | if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
167 | if( EE_Parameter.ServoNickCompInvert & 0x01 ) |
| 167 | else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
168 | ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER ); |
| - | 169 | else |
|
| 168 | if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
170 | ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER ); |
| 169 | else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
- | |
| 170 | 171 | ||
| 171 | DebugOut.Analog[20] = ServoValue; |
172 | DebugOut.Analog[20] = ServoValue; |
| 172 | if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
173 | if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
| 173 | OCR2A = 255-(ServoValue % 256); |
174 | OCR2A = 255-(ServoValue % 256); |
| 174 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
175 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
| 175 | } |
176 | } |