Conrad Quadrocopter umbauen (möglich) ?

Hallo Leute,
ich habe mal wieder ein kleines Zwischenprojekt, da für die anderen Projekte gerade keine Teile vorhanden sind. Also habe ich mir, nachdem ich dieses Video gefunden hatte http://www.youtube.com/watch?v=ny2z6qDWxMI gedacht: Warum bau ich so etwas nicht ? Die Teile habe ich doch fast alle da...

Also habe ich mal angefangen meine Steuerung zwischen Empfänger und Summensignalwandler zu schließen und habe für die Geschwindigkeit mal ein kleines Programm geschrieben. Das hat auch wunderbar funktioniert :)

Code:

---

$regfile = "m32def.dat"
$crystal = 16000000
$baud = 9600
$hwstack = 32
$swstack = 32
$framesize = 32

Config Portd.7 = Output
Config Pind.2 = Input                                      ' Signal 1(a) vom RC-Empfänger
Config Pind.3 = Input
Config Portd.4 = Output

Config Servos = 1 , Servo1 = Portd.4 , Reload = 10

Dim Motor As Word
Dim Kanal_5 As Word

Sound Portd.7 , 400 , 450
Sound Portd.7 , 400 , 250
Sound Portd.7 , 400 , 450
Wait 2
Sound Portd.7 , 400 , 450

Enable Interrupts

Do
  Pulsein Motor , Pind , 2 , 1
  Pulsein Kanal_5 , Pind , 3 , 1

  If Kanal_5 > 100 Then
    Servo(1) = 70
    Wait 1
    Servo(1) = 100
    Wait 1
    Servo(1) = 130
    Wait 1
  End If

  If Kanal_5 < 100 Then
    Servo(1) = Motor
  End If


Loop

End

---

Jetzt möchte ich allerdings auch Sensoren auswerten. Dazu möchte ich einen Infrarotsensor unten an den Q. kleben, der mir zeigt, wie hoch ich schwebe. Auch soll meine Steuerung in der Lage sein, wenn ich einen Knopf drücke, automatisch die Höhe zu halten.

Wenn das funktioniert sollen später auch noch weitere Sensoren wie Gps hinzukommen.
Hier ist mein Code mit dem der Q. automatisch die Höhe halten soll (so habe ich es mir jedenfalls gedacht)

Code:

---

$regfile = "m32def.dat"
$crystal = 16000000
$baud = 9600
$hwstack = 32
$swstack = 32
$framesize = 32

Config Portd.7 = Output                                    'Summer
Config Pind.2 = Input                                      'Motor In
Config Pind.3 = Input                                      'Kanal 5
Config Portd.4 = Output                                    'Motor Out

Config Servos = 1 , Servo1 = Portd.4 , Reload = 10

Config Adc = Single , Prescaler = Auto , Reference = Avcc
Start Adc

Sensor_1 Alias Getadc(0)
Dim D As Word
Dim S As Word
Dim A As Word
Dim I As Integer
Dim Durchschnitt As Word


Dim Motor As Word
Dim Kanal_5 As Word

Dim Gas As Word
Gas = 40

Sound Portd.7 , 400 , 450
Sound Portd.7 , 400 , 250
Sound Portd.7 , 400 , 450
Wait 2
Sound Portd.7 , 400 , 450

Enable Interrupts

Do
  Pulsein Motor , Pind , 2 , 1
  Pulsein Kanal_5 , Pind , 3 , 1

  If Kanal_5 > 100 Then
    Gosub Check_sens_1

    Select Case Durchschnitt

      Case Is < 20
        If Gas < 140 Then
          Gas = Gas + 1
          Servo(1) = Gas
        End If


      Case Is > 30
        If Gas > 50 Then
          Gas = Gas - 1
          Servo(1) = Gas
        End If

    End Select

  End If

  If Kanal_5 < 100 Then
    Servo(1) = Motor
  End If

Loop
End



Check_sens_1:                                              'Berechnen der Höhe in cm und Durchschnitt bilden
  D = 0
  Durchschnitt = 0
  For I = 1 To 10
    S = Sensor_1

    A = S + 22
    D = 5823 / A

    Durchschnitt = Durchschnitt + D
  Next
  Durchschnitt = Durchschnitt / 10
Return

---

Ich würde mich über Kommentare freuen, denn ich weiß nicht ob man das überhaupt so machen kann/darf (Hardware) und ob das so einfach geht wie ich mir das gerade denke ??

Der Einsteiger