$regfile = "m644def.dat"
 $framesize = 32
 $swstack = 32
 $hwstack = 32
 $crystal = 20000000   
 $baud = 9600

 '---------Variablen-------------
 Dim X As Word
 Dim Y As Word
 Dim Temp As Word
 Dim Ledloop As Word
 Dim Motor_soll_v As Integer
 Dim Motor_soll_r As Integer
 Dim Motor_soll_l As Integer
 Dim Motor_soll_h As Integer
 Dim Motor_send_v As Word
 Dim Motor_send_r As Word
 Dim Motor_send_l As Word
 Dim Motor_send_h As Word
 Dim Gyro_nick As Integer
 Dim Gyro_roll As Integer
 Dim Gyro_yaw As Integer
 Dim Besch_x As Integer
 Dim Besch_y As Integer
 Dim Besch_z As Integer
 Dim Gyro_nick_m As Integer
 Dim Gyro_roll_m As Integer
 Dim Gyro_yaw_m As Integer
 Dim Besch_x_m As Integer
 Dim Besch_y_m As Integer
 Dim Besch_z_m As Integer
 Dim Gyro_nick_init As Integer
 Dim Gyro_roll_init As Integer
 Dim Gyro_yaw_init As Integer
 Dim Besch_x_init As Integer
 Dim Besch_y_init As Integer
 Dim Besch_z_init As Integer
 Dim Kanal_nick As Integer
 Dim Kanal_roll As Integer
 Dim Kanal_yaw As Integer
 Dim Kanal_pitch As Integer
 Dim Kanal_nick_init As Integer
 Dim Kanal_roll_init As Integer
 Dim Kanal_yaw_init As Integer
 Dim Kanal_pitch_init As Integer
 Dim Puls(8) As Integer
 Dim Kanal As Byte
 Dim Empfaenger_ok(4) As Byte
 Dim Ok_flag As Bit
 Dim Startflag As Bit
 Dim Kalib_flag As Bit
  '----------Konstanten-----------
 Const Mot_v = &H54                                         'I2C-Adressen der Motoren
 Const Mot_r = &H52
 Const Mot_h = &H56
 Const Mot_l = &H58
 Const Steuerempfindlichkeit = 100
 Const Accempfindlichkeit = 1
 Const Gyroempfindlichkeit = 1
 Const Motorbegrenzung = 130
 Const Motorbegrenzung2 = 150
 Const Delta_x = -10                                        'vorne ist plus
 Const Delta_y = 3                                          'rechts ist plus
 Const Delta_phi = -7                                       'vorne hinten plus -> mehr rechtsdrehen

 '--------Subs------------------
 Declare Sub Entkaefern
 Declare Sub Get_analog
 Declare Sub Set_motoren
 Declare Sub Kallibrieren
 Declare Sub Set_leds
 Declare Sub Werte_pruefen
 Declare Sub Mixer

 '---------Timer----------------
 Config Adc = Single , Prescaler = Auto , Reference = Avcc
 Start Adc

 '-----------Interupts----------
   On Icp1 Get_empfaenger Nosave
   On Ovf1 Kanal_reset Nosave

   Config Timer1 = Timer , Prescale = 8 , Capture Edge = Falling

   Enable Ovf1
   Enable Icp1

   Enable Interrupts

 '---------Ports-----------------
 Config Pinc.2 = Output
 Config Pinc.3 = Output
 Config Pinb.0 = Output
 Config Pinb.1 = Output
 Config Pinb.2 = Output
 Config Pinb.3 = Output
 Config Pinb.4 = Output
 '---------I2C--------------------
 Config Scl = Portc.0                                       'I2C-Ports festlegen
 Config Sda = Portc.1
 I2cinit



 Led_rot Alias Portc.2
 Led_gruen Alias Portc.3
 Led_w_v Alias Portb.0
 Led_w_r Alias Portb.1
 Led_w_h Alias Portb.2
 Led_w_l Alias Portb.3
 Led_blau Alias Portb.4

 '---------Hauptprogramm---------
 'Initwerte
 Startflag = 0
Kanal = 1
Led_gruen = 1
Led_rot = 1
Ledloop = 0
Kallibrieren
Ok_flag = 0
Led_rot = 0
Led_blau = 1
'----------Loop-------------------
Do

Get_analog

Werte_pruefen

Mixer

Set_motoren

Set_leds

'Entkaefern


Loop






'-------- Subs---------------
Sub Entkaefern
   If Startflag = 0 Then
    'Print Ok_flag
    'Print " nick " ; Gyro_nick_m ; " roll " ; Gyro_roll_m ; " yaw " ; Gyro_yaw_m ; " x " ; Besch_x_m ; " y " ; Besch_y_m ; " z " ; Besch_z_m
    'Print " nicki " ; Gyro_nick_init ; " rolli " ; Gyro_roll_init ; " yawi " ; Gyro_yaw_init ; " xi " ; Besch_x_init ; " yi " ; Besch_y_init ; " zi " ; Besch_z_init
    'Print Puls(2) ; " " ; Puls(3) ; " " ; Puls(4) ; " " ; Puls(5) ; " " ; Puls(6) ; " " ; Puls(7) ; " "
    'Print "k_nick " ; Kanal_nick ; " k_roll " ; Kanal_roll ; " k_yaw " ; Kanal_yaw ; " k_pitch " ; Kanal_pitch
    'Print Ledloop
    'Print "vorne: " ; Motor_send_v ; "         rechts: " ; Motor_send_r ; "         links: " ; Motor_send_l ; "         hinten: " ; Motor_send_h
    'Print "motor-v: " ; Motor_send_v ; " motor_h: " ; Motor_send_h ; " nick " ; Gyro_nick ; " x " ; Besch_x
    'Print "motor-r: " ; Motor_send_r ; " motor_l: " ; Motor_send_l ; " roll " ; Gyro_roll ; " y " ; Besch_y
    End If
End Sub

'-------------------------------------------------------------------------------
Sub Get_analog

   Gyro_nick = Getadc(7) - Gyro_nick_init
   Gyro_roll = Getadc(4) - Gyro_roll_init
   Gyro_yaw = Getadc(5) - Gyro_yaw_init
   Besch_x = Getadc(2) - Besch_x_init
   Besch_y = Getadc(1) - Besch_y_init
   Besch_z = Getadc(3) - Besch_z_init

   Gyro_nick_m = Gyro_nick_m * 8
   Gyro_nick_m = Gyro_nick_m + Gyro_nick
   Gyro_nick_m = Gyro_nick_m / 9

   Gyro_roll_m = Gyro_roll_m * 8
   Gyro_roll_m = Gyro_roll_m + Gyro_roll
   Gyro_roll_m = Gyro_roll_m / 9

   Gyro_yaw_m = Gyro_yaw_m * 8
   Gyro_yaw_m = Gyro_yaw_m + Gyro_yaw
   Gyro_yaw_m = Gyro_yaw_m / 9

   Besch_x_m = Besch_x_m * 40
   Besch_x_m = Besch_x_m + Besch_x
   Besch_x_m = Besch_x_m / 42

   Besch_y_m = Besch_y_m * 30
   Besch_y_m = Besch_y_m + Besch_y
   Besch_y_m = Besch_y_m / 42

   Besch_z_m = Besch_z_m * 40
   Besch_z_m = Besch_z_m + Besch_z
   Besch_z_m = Besch_z_m / 42

   Gyro_nick_m = Gyro_nick_m * Gyroempfindlichkeit
   Gyro_roll_m = Gyro_roll_m * Gyroempfindlichkeit
   Gyro_yaw_m = Gyro_yaw_m * Gyroempfindlichkeit
   Besch_x_m = Besch_x_m * Accempfindlichkeit
   Besch_y_m = Besch_y_m * Accempfindlichkeit
   Besch_z_m = Besch_z_m * Accempfindlichkeit

End Sub

'-------------------------------------------------------------------------------
Sub Werte_pruefen
 Ok_flag = 1
 For X = 2 To 5
     Select Case Puls(x)
     Case -23000 To -19000 :
     Case Else : Ok_flag = 0
 End Select
 If Kalib_flag = 0 Then
   Ok_flag = 0
 End If

 Next X
  Kanal_nick = Puls(3) - Kanal_nick_init
  Kanal_roll = Puls(2) - Kanal_roll_init
  Kanal_yaw = Puls(4) - Kanal_yaw_init
  Kanal_pitch = Puls(5) - Kanal_pitch_init
  Kanal_nick = Kanal_nick / Steuerempfindlichkeit
  Kanal_roll = Kanal_roll / Steuerempfindlichkeit
  Kanal_yaw = Kanal_yaw / Steuerempfindlichkeit
  Kanal_pitch = Kanal_pitch / 10
End Sub

'-------------------------------------------------------------------------------
Sub Mixer

If Kanal_pitch < -50 Then
    Startflag = 1
    If Kanal_yaw < -2 Then
         Startflag = 0
    End If
End If

Motor_soll_v = Kanal_pitch + 40                             'pitch
Motor_soll_r = Kanal_pitch + 40
Motor_soll_h = Kanal_pitch + 40
Motor_soll_l = Kanal_pitch + 40
Motor_soll_v = Motor_soll_v + Delta_x                       'feinjustierung
Motor_soll_r = Motor_soll_r + Delta_y
Motor_soll_h = Motor_soll_h - Delta_x
Motor_soll_l = Motor_soll_l - Delta_y
Motor_soll_v = Motor_soll_v + Delta_phi                     'feinjustierung
Motor_soll_r = Motor_soll_r - Delta_phi
Motor_soll_h = Motor_soll_h + Delta_phi
Motor_soll_l = Motor_soll_l - Delta_phi


If Motor_soll_v > Motorbegrenzung Then
   Motor_soll_v = Motorbegrenzung
End If

If Motor_soll_r > Motorbegrenzung Then
   Motor_soll_r = Motorbegrenzung
End If

If Motor_soll_h > Motorbegrenzung Then
   Motor_soll_h = Motorbegrenzung
End If

If Motor_soll_l > Motorbegrenzung Then
   Motor_soll_l = Motorbegrenzung
End If

Motor_soll_v = Motor_soll_v - Kanal_nick                    'nick
Motor_soll_h = Motor_soll_h + Kanal_nick
Motor_soll_v = Motor_soll_v + Gyro_nick_m                   'gyro nick
Motor_soll_h = Motor_soll_h - Gyro_nick_m
Motor_soll_v = Motor_soll_v + Besch_x_m                     'acc nick
Motor_soll_h = Motor_soll_h - Besch_x_m

Motor_soll_r = Motor_soll_r - Kanal_roll                    'roll
Motor_soll_l = Motor_soll_l + Kanal_roll
Motor_soll_r = Motor_soll_r - Gyro_roll_m                   'gyro roll
Motor_soll_l = Motor_soll_l + Gyro_roll_m
Motor_soll_r = Motor_soll_r + Besch_y_m                     'acc roll
Motor_soll_l = Motor_soll_l - Besch_y_m

'Motor_soll_v = Motor_soll_v - Kanal_yaw                     'yaw
'Motor_soll_l = Motor_soll_l + Kanal_yaw
'Motor_soll_r = Motor_soll_r + Kanal_yaw
'Motor_soll_h = Motor_soll_h - Kanal_yaw

If Motor_soll_v < 1 Then
   Motor_soll_v = 1
End If

If Motor_soll_r < 1 Then
   Motor_soll_r = 1
End If

If Motor_soll_h < 1 Then
   Motor_soll_h = 1
End If

If Motor_soll_l < 1 Then
   Motor_soll_l = 1
End If


Motor_send_v = Motor_soll_v
Motor_send_r = Motor_soll_r
Motor_send_h = Motor_soll_h
Motor_send_l = Motor_soll_l


End Sub


'-------------------------------------------------------------------------------
Sub Set_motoren
                                       ' Werte an Motoren schicken

   If Motor_send_v < 5 Then
          Motor_send_v = 5
   End If
   If Motor_send_v > Motorbegrenzung2 Then
          Motor_send_v = Motorbegrenzung2
   End If
   If Motor_send_h < 5 Then
          Motor_send_h = 5
   End If
   If Motor_send_h > Motorbegrenzung2 Then
          Motor_send_h = Motorbegrenzung2
   End If
   If Motor_send_r < 5 Then
          Motor_send_r = 5
   End If
   If Motor_send_r > Motorbegrenzung2 Then
          Motor_send_r = Motorbegrenzung2
   End If
   If Motor_send_l < 5 Then
          Motor_send_l = 5
   End If
   If Motor_send_l > Motorbegrenzung2 Then
          Motor_send_l = Motorbegrenzung2
   End If
If Ok_flag = 1 Then
   If Startflag = 1 Then
      I2csend Mot_v , Motor_send_v
      For X = 1 To 120
      Next X

      I2csend Mot_h , Motor_send_h
      For X = 1 To 120
      Next X

      I2csend Mot_r , Motor_send_r
      For X = 1 To 120
      Next X

      I2csend Mot_l , Motor_send_l
      For X = 1 To 120
      Next X

   End If
End If

End Sub

'-------------------------------------------------------------------------------
Sub Kallibrieren
   For X = 1 To 1000                                        ' ADCs aufwäremen :)
            For Y = 0 To 7
            Temp = Getadc(x)
            Next Y
   Next X
   Wait 1
   Gyro_nick_init = 0
   Gyro_roll_init = 0
   Gyro_yaw_init = 0
   Besch_x_init = 0
   Besch_y_init = 0
   Besch_z_init = 0
   Kanal_nick_init = 0
   Kanal_roll_init = 0
   Kanal_yaw_init = 0
   Kanal_pitch_init = 0
   For X = 1 To 100
      Gyro_nick_init = Gyro_nick_init + Getadc(7)
      Gyro_roll_init = Gyro_roll_init + Getadc(4)
      Gyro_yaw_init = Gyro_yaw_init + Getadc(5)
      Besch_x_init = Besch_x_init + Getadc(2)
      Besch_y_init = Besch_y_init + Getadc(1)
   Next X
   Gyro_nick_init = Gyro_nick_init / 100
   Gyro_roll_init = Gyro_roll_init / 100
   Gyro_yaw_init = Gyro_yaw_init / 100
   Besch_x_init = Besch_x_init / 100
   Besch_y_init = Besch_y_init / 100
   Besch_z_init = Besch_x_init + Besch_y_init
   Besch_z_init = Besch_z_init / 2

    Kalib_flag = 1
    For Y = 1 To 100
       For X = 2 To 5
           Select Case Puls(x)
           Case -23000 To -19000 :
           Case Else : Kalib_flag = 0
       End Select
       Next X
    Next Y

    If Kalib_flag = 1 Then
        Kanal_nick_init = Puls(3)
        Kanal_roll_init = Puls(2)
        Kanal_yaw_init = Puls(4)
        Kanal_pitch_init = Puls(5)
    End If
End Sub



'-------------------------------------------------------------------------------

Sub Set_leds
   If Ok_flag = 1 Then
      If Ledloop = 2000 Then
         Led_w_v = 1
         Led_w_r = 1
         Led_w_h = 1
         Led_w_l = 1
      Elseif Ledloop = 2100 Then
         Led_w_v = 0
         Led_w_r = 0
         Led_w_h = 0
         Led_w_l = 0
         Ledloop = 0
      End If
   Else
    If Ledloop = 1000 Then
         Led_w_v = 1
         Led_w_r = 1
         Led_w_h = 1
         Led_w_l = 1
      Elseif Ledloop = 2000 Then
         Led_w_v = 0
         Led_w_r = 0
         Led_w_h = 0
         Led_w_l = 0
         Ledloop = 0
      End If

   End If
   Incr Ledloop
End Sub
'------------Interupts---------------
 Get_empfaenger:
      push r10
      push r11
      push r24
      push r25
      push r26
      push r27
      in r24,sreg
      push r24

      Puls(kanal) = Timer1
      Timer1 = 40000

      Incr Kanal

      pop r24
      !Out Sreg , R24
      pop r27
      pop r26
      pop r25
      pop r24
      pop r11
      pop r10



     Return


Kanal_reset:


    push r24
    in r24,sreg
    push r24

    Kanal = 1
    pop r24
    !Out sreg,r24
    pop r24
   Return

End