Prob mit servo timer und adc

[mki]

hi,
habe ein prob. mit den Servos in bascom, der irq wird zwar ausgelöst aber inerhalb des irqs funktionieren die Servos nicht

$regfile = "m32def.dat"
$crystal = 14745600
$baud = 115200

$framesize = 32
$swstack = 256
$hwstack = 128


Config Timer1 = Timer , Prescale = 256
On Timer1 Ontimer1
Enable Timer1
Timer1 = 7935

'servo
Config Servos = 2 , Servo1 = Portc.2 , Servo2 = Portc.3 , Reload = 10       ', Timer = Timer2
Config Pinc.3 = Output
Config Pinc.2 = Output

'ADC
Config Adc = Single , Prescaler = Auto , Reference = Avcc  , Timer = Timer2

Config Pinb.0 = Output


Enable Interrupts

Declare Sub Stopleft
Declare Sub Stopright
Declare Sub Stopfull
Declare Sub Forwardleft
Declare Sub Forwardright
Declare Sub Rewardleft
Declare Sub Rewardright
Declare Sub Turnleft
Declare Sub Turnright

Declare Sub Ausweichen

Dim Dist1 As Integer
Dim Speed As Integer
Speed = 50

Stopfull


'Now give power to the chip
Start Adc

'Waitms 5000

'Forwardleft
'Forwardright
Waitms 2000
Stopfull

Speed = 10
Do

'      W = Getadc(0)

      Forwardleft
      Waitms 2250
      Stopfull
      Forwardright
      Waitms 2250
      Stopfull
      Forwardleft
      Forwardright
      Waitms 2000
      Stopfull
      Turnleft
      Waitms 2000
      Stopfull

   Waitms 2000
   Stopfull

Loop

End

Sub Ausweichen
      Dist1 = Getadc(0)
      Print "0 value: " ; Dist1
      If Dist1 > 19 Then
      Print "trunright " ; Dist1
'         Stopfull
         Turnright
         Portb.0 = 1

         Do
            Waitms 500
            Dist1 = Getadc(0)
            Print "ausweichen: " ; Dist1
            If Dist1 < 19 Then
               Stopfull
               Portb.0 = 0
               Return
            Else
               Turnright
            End If
         Loop
      End If
End Sub

OnTimer1:                                                   'Interrupt-Routine
   Timer1 = 7935
   Ausweichen
   Return



Sub Stopleft
   Servo(1) = 100
End Sub

Sub Stopright
   Servo(2) = 100
End Sub

Sub Stopfull
   Servo(1) = 100
   Servo(2) = 100
End Sub

Sub Forwardleft
   Servo(1) = Speed + 100
End Sub

Sub Forwardright
   Servo(2) = 100 - Speed
End Sub

Sub Rewardleft
   Servo(1) = 100 - Speed
End Sub

Sub Rewardright
   Servo(2) = Speed + 100
End Sub


Sub Turnleft
   Servo(1) = 100 - Speed
   Servo(2) = 100 - Speed
End Sub

Sub Turnright
   Servo(1) = Speed + 100
   Servo(2) = Speed + 100
End Sub