Genetischer Algorithmus für QuadroCopter

[Bammel]

Hallo,

gibt es schon neuigkeiten?

Gruß, Bammel

[Che Guevara]

Hallo,

nein, leider nicht! Ich hatte eine E-Mail an TT-Copter geschrieben und gefragt, ob sie mir freundlicherweise sagen könnten, mit welchem Verfahren ihr Copter seine Parameter optimiert. Aber anscheinend ist das ein Geheimnis .... Habe nämlich keine E-Mail erhalten.
Mit dem Prüfstandaufbau kann ich auch noch nicht beginnen, da ich derzeit den Feinschliff meiner FA mache und nach den Ferien auch noch ein paar Klausuren schreibe. Ich denke, vor Weihnachten wird sich da nicht allzuviel ergeben, Schule geht vor.
Aber sobald ich weitergemacht habe, poste ich hier natürlich meine Erfahrungen.

Gruß
Chris

[Che Guevara]

Hallo,

nun, da wieder 2 Klausuren vorbei sind, habe ich wieder Zeit, an dem Projekt weiterzumachen.
Zuerst habe ich allerdings eine Frage; folgender Code:

if (fcandidate<f0) T*=a;
w0=wcandidate;
f0=fcandidate;

Wird das so:

if fcandidate < f0 then
    temperatur = temperatur * t_a
    w0_kp = wcandidate_kp
    w0_ki = wcandidate_ki
    w0_kd = wcandidate_kd
    f0 = fcandidate
endif

oder so:

if fcandidate < f0 then
    temperatur = temperatur * t_a
endif
 w0_kp = wcandidate_kp
w0_ki = wcandidate_ki
w0_kd = wcandidate_kd
f0 = fcandidate

nach BASIC übersetzt?
Mich verwirrt diese IF-Abfrage. C habe ich nie gelernt, nur in der Schule etwas JAVA, aber ich denke, das sollte in etwa eine ähnliche Syntax haben?!
Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?

Mittlerweile habe ich auch schon versucht, den kompletten Code anzupassen, hier mal mein aktuelles Geschreibsel:

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 250
$hwstack = 250
$swstack = 250
$baud = 38400



Config Serialin = Buffered , Size = 100
Config Serialout = Buffered , Size = 100


Declare Sub Simulated_annealing(byval N As Byte)

Declare Function Getfitness() As Single

Declare Sub Log_data()

Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.28                                              '0.32
_roll_kp = 0.28                                             '0.32
_pitch_kp = 0.25                                            '0.32

_yaw_ki = 0.0001                                            '0.000135
_roll_ki = 0.0001                                           '0.000135
_pitch_ki = 0.0001                                          '0.000135

_yaw_kd = 0.000007                                          '0.0000096
_roll_kd = 0.000007                                         '0.0000096
_pitch_kd = 0.000007                                        '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte



Dim Extmp As Single                                         'Integer
Dim Eytmp As Single                                         'Integer
Dim Eztmp As Single                                         'Integer
Dim Ax(50) As Single
Dim Ay(50) As Single
Dim Az(50) As Single
Dim Ex As Single                                            'Integer
Dim Ey As Single                                            'Integer
Dim Ez As Single                                            'Integer

Dim Atmp As Byte
Dim A As Byte

Dim F As Single



Dim Fcandidate As Single
Dim Kp_mom As Single
Dim Ki_mom As Single
Dim Kd_mom As Single
Dim Temperatur As Single
Dim T_a As Single
Dim Fp As Single
Dim Fi As Single
Dim Fd As Single
Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim Wcandidate_kp As Single
Dim Wcandidate_ki As Single
Dim Wcandidate_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single
Dim F0 As Single


Dim Test As Word                                            'Byte


Dim Log_cnt As Byte

Dim Send_logged_data As Byte
Dim Rx_in As String * 30
'Dim Var(50) As Byte

Dim Kanal_6_old As Byte

Dim B As String * 1
Dim Lenght As Byte


Dim Itmp As Byte
Dim Itmp_old As Byte

Dim Xtmp As Byte



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
   If Rc_on < 65000 Then Incr Rc_on
Else
   If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
   Goto Rc_test
End If


'_bl(1) = rechts hinten  (US)
'_bl(2) = links hinten   (GUS)
'_bl(3) = rechts vorne   (GUS)
'_bl(4) = links vorne    (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms



   Wcandidate_kp = 0.3
   Wcandidate_ki = 0.0001
   Wcandidate_kd = 0.000007
   Temperatur = 1.0
   T_a = 0.8                                                '0.95
   Fp = 0.1                                                 '0.1
   Fi = 0.0001
   Fd = 0.000005

   Fcandidate = 500                                         '150
   'Fcandidate = Getfitness()
   W0_kp = Wcandidate_kp
   W0_ki = Wcandidate_ki
   W0_kd = Wcandidate_kd
   F0 = Fcandidate





Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   _bl(1) = 113
   _bl(4) = 113
   Call Set_pwm()


   If State > 0 Then

      Incr Test
      If Itmp < 32 Then
         If Itmp > Itmp_old Then

            Rp_i = Rnd(20) - 10
            Rp = Rp_i / 10
            Wi_kp = Temperatur * Fp
            Wi_kp = Wi_kp * Rp
            Wi_kp = Wi_kp + W0_kp

            Ri_i = Rnd(20) - 10
            Ri = Ri_i / 10
            Wi_ki = Temperatur * Fi
            Wi_ki = Wi_ki * Ri
            Wi_ki = Wi_ki + W0_ki

            Rd_i = Rnd(20) - 10
            Rd = Rd_i / 10
            Wi_kd = Temperatur * Fd
            Wi_kd = Wi_kd * Rd
            Wi_kd = Wi_kd + W0_kd

            If Wi_kp < 0 Then Wi_kp = 0
            If Wi_ki < 0 Then Wi_ki = 0
            If Wi_kd < 0 Then Wi_kd = 0                     '#######

            _roll_kp = Wi_kp
            _pitch_kp = Wi_kp
            _roll_ki = Wi_ki
            _pitch_ki = Wi_ki
            _roll_kd = Wi_kd
            _pitch_kd = Wi_kd

         End If

         Itmp_old = Itmp

         Incr Xtmp
         If Xtmp = 31 Then
            Incr Itmp
            Xtmp = 0
            F = Getfitness()
            If F < Fcandidate Then
               Fcandidate = F
               Wcandidate_kp = Wi_kp
               Wcandidate_ki = Wi_ki
               Wcandidate_kd = Wi_kd
            End If
         Elseif Xtmp < 31 Then
            Ax(xtmp) = _roll_err
         End If

      End If

      If Itmp = 32 Then
         Itmp = 0
         If Fcandidate < F0 Then
            Temperatur = Temperatur * T_a
            W0_kp = Wcandidate_kp
            W0_ki = Wcandidate_ki
            W0_kd = Wcandidate_kd
            F0 = Fcandidate
         End If
      End If

   Elseif State = 0 Then

      If Ischarwaiting() > 0 Then
         Input Rx_in Noecho
         B = Left(rx_in , 1)
         If Asc(b) = 10 Or Asc(b) = 13 Then
            Lenght = Len(rx_in)
            Lenght = Lenght - 1
            Rx_in = Right(rx_in , Lenght)
            Clear Serialin                                  '######
         End If
         Send_logged_data = Instr(rx_in , "send!")
         If Send_logged_data = 1 Then
            Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
            Test = 0
            Send_logged_data = 0
            Clear Serialin                                  '######
         End If
      End If

   End If


Loop


Sub Simulated_annealing(byval N As Byte)
   Wcandidate_kp = 0.3
   Wcandidate_ki = 0.0001
   Wcandidate_kd = 0.000007
   Temperatur = 1.0
   T_a = 0.90
   Fp = 0.1
   Fi = 0.0002
   Fd = 0.000005
   Fcandidate = Getfitness()
   W0_kp = Wcandidate_kp
   W0_ki = Wcandidate_ki
   W0_kd = Wcandidate_kd
   F0 = Fcandidate
   Test = 0
   While Fcandidate > 50
      Incr Test
      For I = 1 To N
         Rp_i = Rnd(20) - 10
         Rp = Rp_i / 10                                     '################
         Wi_kp = Temperatur * Fp
         Wi_kp = Wi_kp * Rp
         Wi_kp = Wi_kp + W0_kp

         Ri_i = Rnd(20) - 10
         Ri = Ri_i / 10
         Wi_ki = Temperatur * Fi
         Wi_ki = Wi_ki * Ri
         Wi_ki = Wi_ki + W0_ki

         Rd_i = Rnd(20) - 10
         Rd = Rd_i / 10
         Wi_kd = Temperatur * Fd
         Wi_kd = Wi_kd * Rd
         Wi_kd = Wi_kd + W0_kd

         F = Getfitness()
         If F < Fcandidate Then
            Fcandidate = F
            Wcandidate_kp = Wi_kp
            Wcandidate_ki = Wi_ki
            Wcandidate_kd = Wi_kd
         End If
      Next I
      If Fcandidate < F0 Then
         Temperatur = Temperatur * T_a
      'End If
         W0_kp = Wcandidate_kp                              '#########
         W0_ki = Wcandidate_ki
         W0_kd = Wcandidate_kd
         F0 = Fcandidate
      End If
      'Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
   Wend
End Sub


Sub Log_data()
   If State > 0 Then
      Incr Log_cnt
      If Log_cnt < 30 Then
         Ax(log_cnt) = _rollnow
         'Ay(log_cnt) = _pitchnow
         'Az(log_cnt) = _yawnow
      Elseif Log_cnt = 250 Then
         Log_cnt = 200
      End If
   Elseif State = 0 Then
      If Ischarwaiting() > 0 Then
         Input Rx_in Noecho
         B = Left(rx_in , 1)
         If Asc(b) = 10 Or Asc(b) = 13 Then
            Lenght = Len(rx_in)
            Lenght = Lenght - 1
            Rx_in = Right(rx_in , Lenght)
         End If
         Send_logged_data = Instr(rx_in , "send!")
         If Send_logged_data = 1 Then
            F = Getfitness()
            Print F
            Send_logged_data = 0
         End If
      End If
   End If
End Sub


Function Getfitness() As Single
   'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2
   Ex = 0
   Ey = 0
   Ez = 0
   For A = 1 To 30
      If A > 1 Then
         Atmp = A - 1
      Else
         Atmp = 30
      End If
      Extmp = Ax(a) - Ax(atmp)
      Extmp = Extmp * Extmp
      Eytmp = Ay(a) - Ay(atmp)
      Eytmp = Eytmp * Eytmp
      Eztmp = Az(a) - Az(atmp)
      Eztmp = Eztmp * Eztmp
      Ex = Ex + Extmp
      Ey = Ey + Eytmp
      Ez = Ez + Eztmp
   Next A
   Getfitness = Ex + Ey
   Getfitness = Getfitness + Ez
End Function


Sub Set_pwm()
   If _bl(1) > 250 Then _bl(1) = 250
   If _bl(1) < 113 Then _bl(1) = 113
   If _bl(2) > 250 Then _bl(2) = 250
   If _bl(2) < 113 Then _bl(2) = 113
   If _bl(3) > 250 Then _bl(3) = 250
   If _bl(3) < 113 Then _bl(3) = 113
   If _bl(4) > 250 Then _bl(4) = 250
   If _bl(4) < 113 Then _bl(4) = 113
   Pwm2a = _bl(4)                                           'links vorne (US)
   Pwm2b = _bl(2)                                           'links hinten (GUS)
   Pwm1a = _bl(3)                                           'rechts vorne (GUS)
   Pwm1b = _bl(1)                                           'rechts hinten (US)
End Sub


Sub Failsave()
  If Channel >= 11 Then
    If Failure < 255 Then
      Incr Failure
    End If
  End If
  If Channel < 11 Then
    If Failure > 0 Then
      Decr Failure
    End If
  End If
End Sub


Measure:
 If Channel > 0 And Channel < 9 Then
   Kanal(channel) = Timer0
 End If
 Timer0 = 6
 Incr Channel
Return


Pausedetect:
  If Channel <> 0 Then
    Newvalsreceived = 1
  End If
  Channel = 0
Return


Sub Filter_gyro_data()
 Call Read_wmp_data()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

 If Newvalsreceived = 1 Then
   Newvalsreceived = 0

   For I = 1 To Maxchannel
     Fkanal(i) = Fkanal(i) * 3
     Fkanal(i) = Fkanal(i) + Kanal(i)
     Shift Fkanal(i) , Right , 2
     Hempf(i) = Fkanal(i) + 17
     Lempf(i) = Fkanal(i) - 17
     If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
        Kanal(i) = Fkanal(i)
     End If
   Next I

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
     If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
     If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
   End If

   For I = 2 To Maxchannel
     If Kanal(i) >= 62 And Kanal(i) < 139 Then
       _sbl(i) = Kanal(i) - 100
     End If
   Next I

   If Kanal(5) < 80 Then State = 0
   If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
   If Kanal(5) >= 115 Then State = 2


   Setpoint_yaw = _sbl(_yawchannel) * 1.5

   If State < 2 Then
      Setpoint_roll = _sbl(_rollchannel) * 0.6
      Setpoint_pitch = _sbl(_pitchchannel) * 0.6
   Else
      Setpoint_roll = _sbl(_rollchannel) * 1.5
      Setpoint_pitch = _sbl(_pitchchannel) * 1.5
   End If


   If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
     Call Set_wmp_offset()
   End If

   Oldstate = State

 End If

End Sub


Sub Wmp_init()
   I2cstart
   I2cwbyte &HA6
   I2cwbyte &HFE
   I2cwbyte &H04
   I2cstop
End Sub


Sub Send_zero()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
End Sub


Sub Read_wmp_data()
   Gosub Send_zero
   I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
   Yaw1 = Wmplus_buffer(1)
   Roll1 = Wmplus_buffer(2)
   Pitch1 = Wmplus_buffer(3)
   Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
   Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
   Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
   Shift Yaw , Right , 4
   Shift Roll , Right , 4
   Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_wmp_data()
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
   _yawoffset_int = _yawoffset
   _rolloffset_int = _rolloffset
   _pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
 _yaw_err = Setpoint_yaw - _yawnow
 _yaw_kp_err = _yaw_err * _yaw_kp
 _yaw_ki_sum = _yaw_ki_sum + _yaw_err
 If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
 _yaw_ki_err = _yaw_ki_sum * _yaw_ki
 _yaw_kd_err = _yaw_err - _yaw_kd_old
 _yaw_kd_old = _yaw_err
 _yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
 _roll_err = Setpoint_roll - _rollnow
 _roll_kp_err = _roll_err * _roll_kp
 _roll_ki_sum = _roll_ki_sum + _roll_err
 If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
 _roll_ki_err = _roll_ki_sum * _roll_ki
 _roll_kd_err = _roll_err - _roll_kd_old
 _roll_kd_old = _roll_err
 _roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
 _pitch_err = Setpoint_pitch - _pitchnow
 _pitch_kp_err = _pitch_err * _pitch_kp
 _pitch_ki_sum = _pitch_ki_sum + _pitch_err
 If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
 _pitch_ki_err = _pitch_ki_sum * _pitch_ki
 _pitch_kd_err = _pitch_err - _pitch_kd_old
 _pitch_kd_old = _pitch_err
 _pitch_kd_err = _pitch_kd_err * _pitch_kd


 _yaw_pid = _yaw_kp_err + _yaw_ki_err
 _yaw_pid = _yaw_pid + _yaw_kd_err
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_err
 _roll_pid = _roll_pid + _roll_kd_err
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_err
 _pitch_pid = _pitch_pid + _pitch_kd_err
 _pitch_pid_int = _pitch_pid

 If _yaw_pid_int < -68 Then _yaw_pid_int = -68
 If _yaw_pid_int > 68 Then _yaw_pid_int = 68
 If _roll_pid_int < -68 Then _roll_pid_int = -68
 If _roll_pid_int > 68 Then _roll_pid_int = 68
 If _pitch_pid_int < -68 Then _pitch_pid_int = -68
 If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

  '_bl(1) = rechts hinten  (US)
  '_bl(2) = links hinten   (GUS)
  '_bl(3) = rechts vorne   (GUS)
  '_bl(4) = links vorne    (US)

 _bl(1) = 182 + _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = _bl(1)

 If State <> 0 And Failure < 20 Then

   Portc.2 = 0
   Portc.3 = 1

   _bl(1) = _bl(1) + Minthrottle
   _bl(2) = _bl(2) + Minthrottle
   _bl(3) = _bl(3) + Minthrottle
   _bl(4) = _bl(4) + Minthrottle

   _bl(1) = _bl(1) - _pitch_pid_int
   _bl(4) = _bl(4) + _pitch_pid_int

   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int

   _bl(1) = _bl(1) - _yaw_pid_int
   _bl(2) = _bl(2) + _yaw_pid_int
   _bl(3) = _bl(3) + _yaw_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int

 Else

   Portc.2 = 0

   _bl(1) = 113
   _bl(2) = 113
   _bl(3) = 113
   _bl(4) = 113

   Incr _blink
   If _blink = 100 Then
     Portc.3 = 1
     _blink = 101
   End If
   If _blink = 200 Then
     Portc.3 = 0
     _blink = 0
   End If

   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err = 0
   _pitch_ki_err = 0
   _yaw_pid_int = 0
   _roll_pid_int = 0
   _pitch_pid_int = 0
   _yaw_pid = 0
   _roll_pid = 0
   _pitch_pid = 0
   Yawstickvel = 0
   Rollstickvel = 0
   Pitchstickvel = 0
   Yawstickold = 0
   Rollstickold = 0
   Pitchstickold = 0

 End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Ich weiß, manche Variablen haben komische Namen, das wird noch ausgebessert. Zur Info: XTMP und ITMP sind reine Zählvariablen. Vielleicht hat jemand von euch ja die Geduld, sich das ganze mal kurz anzusehen (also nur / hauptsächlich den Simulated-Annealing-Teil im Main-loop)?!

Vielen Dank & Gruß
Chris

EDIT:
Kennt jemand ein gutes Buch, in dem ausführlich auf sowohl Theorie, als auch Praxis des Simulated Annealing eingegangen wird? Am besten mit ein paar Programmcodes als Beispiel ... Wäre nett

EDIT2:
Hab mich jetzt selbst mal an einem SA versucht, vielleicht wäre jemand ja mal so nett, um kurz drüberzuschauen, ob grobe Fehler drin sind.

$regfile = "m32def.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 38400



Declare Function Getfitness() As Single



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Single
Dim F0 As Single

'------evtl. als long!!!------------------------------
Dim Extmp As Single                                         'Integer
Dim Eytmp As Single                                         'Integer
Dim Eztmp As Single                                         'Integer
Dim Ax(50) As Single                                        'Integer
Dim Ay(50) As Single                                        'Integer
Dim Az(50) As Single                                        'Integer
Dim Ex As Single                                            'Integer
Dim Ey As Single                                            'Integer
Dim Ez As Single                                            'Integer

Dim Atmp As Word
Dim A As Word

Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word


Dim _roll_kp As Single
Dim _roll_ki As Single
Dim _roll_kd As Single

Dim _rollnow As Integer

Dim X As Integer


'-------initialize the values
T = 1.0
T_a = 0.98
F0 = 500.0
F = F0
W0_kp = 5
W0_ki = 0.1
W0_kd = 0.043
Fp = 2
Fi = 0.08
Fd = 0.005
Max_log_cnt = 15
Max_tries = 250



Enable Interrupts



Do


   '-------randomize according to the current temperature--------
   Rp_i = Rnd(200) - 100
   Rp = Rp_i * 0.01
   Rp = Rp * T
   Ri_i = Rnd(200) - 100
   Ri = Ri_i * 0.01
   Ri = Ri * T
   Rd_i = Rnd(200) - 100
   Rd = Rd_i * 0.01
   Rd = Rd * T

   '-------generate new parameters------------------------------
   Wi_kp = Rp * Fp
   Wi_kp = Wi_kp + W0_kp
   Wi_ki = Ri * Fi
   Wi_ki = Wi_ki + W0_ki
   Wi_kd = Rd * Fd
   Wi_kd = Wi_kd + W0_kd

   '-------test current parameters------------------------------
   _roll_kp = Wi_kp
   _roll_ki = Wi_ki
   _roll_kd = Wi_kd

   '-------log data for fitness function------------------------
   Incr Log_cnt
   If Log_cnt < Max_log_cnt Then
      Ax(log_cnt) = _rollnow                                '################################################################################
   Elseif Log_cnt = Max_log_cnt Then
      F = Getfitness()
      Log_cnt = 0
   End If

   '-------better then current solution?------------------------
   If F < F0 Then
      F0 = F
      W0_kp = Wi_kp
      W0_ki = Wi_ki
      W0_kd = Wi_kd
      T = T * T_a
      Print Tries ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
   End If

   '-------counter for max tries---------------------------------
   Incr Tries


Loop


Function Getfitness() As Single
   'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2
   Ex = 0
   Ey = 0
   Ez = 0
   For A = 1 To Max_log_cnt
      If A > 1 Then
         Atmp = A - 1
      Else
         Atmp = Max_log_cnt
      End If
      Extmp = Ax(a) - Ax(atmp)
      Extmp = Extmp * Extmp
      Eytmp = Ay(a) - Ay(atmp)
      Eytmp = Eytmp * Eytmp
      Eztmp = Az(a) - Az(atmp)
      Eztmp = Eztmp * Eztmp
      Ex = Ex + Extmp
      Ey = Ey + Eytmp
      Ez = Ez + Eztmp
   Next A
   Getfitness = Ex + Ey
   Getfitness = Getfitness + Ez
End Function

End

[021aet04]

Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?

Hinter der IF Abfrage ist keine {-Klammer weil nur ein Kommando ("T*=a") steht.
Man könnte stattdessen auch "if (fcandidate<f0) { T*=a;}" schreiben.
Ist aber das gleiche. Wenn man mehrere Befehle/Kommandos hat muss man eine {-Klammer schreiben.

MfG Hannes

[Bernina]

Tip: Ich kenne mich zwar mit Optimierungsmethoden wenig aus, weiß aber, dass MATLAB eine Menge Algorithmen hierfür bereithält.

[Che Guevara]

Oh... ok
Ist zwar jetzt sowieso nicht mehr relevant, da ich meinen eigenen Algorithmus geschrieben habe, aber trotzdem danke, Wissen ist nie verkehrt!

Hm also eigentlich soll der Algorithmus später dann die ganze Zeit auf dem Quadrocopter mitlaufen, damit die Parameter ständig angepasst werden können, falls nötig. Deswegen muss ich auf einen PC usw.. verzichten. Trotzdem danke für den Tipp.

Heute werde ich mal, sofern ich Zeit finde, meinen Algorithmus testen und sehen, was dabei rauskommt. Sobald ich Ergebnisse habe, poste ich sie hier.

Gruß
Chris

EDIT:
Hier ist jetzt mal mein aktueller Code:

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 100
$hwstack = 100
$swstack = 100
$baud = 38400


Declare Sub Serial0charmatch()

Config Serialin = Buffered , Size = 30 , Bytematch = 13


Declare Sub Simulated_annealing()



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Double
Dim F0 As Double

'------evtl. als long!!!------------------------------
Dim Extmp As Double
Dim Eytmp As Double
Dim Eztmp As Double
Dim Ax(50) As Double
Dim Ay(50) As Double
Dim Az(50) As Double
Dim Ex As Double
Dim Ey As Double
Dim Ez As Double

Dim Atmp As Word
Dim A As Word

Dim Log_step As Byte
Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word

Dim Test_anz As Long


Dim X As Integer



'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################
Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.4                                               '0.32
_roll_kp = 0.4                                              '0.32
_pitch_kp = 0.4                                             '0.32

_yaw_ki = 0.0008                                            '0.0008
_roll_ki = 0.0015                                           '0.0018
_pitch_ki = 0.0015                                          '0.0018

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0

'_yaw_ki = 0.0001                                            '0.000135
'_roll_ki = 0.0001                                           '0.000135
'_pitch_ki = 0.0001                                          '0.000135

'_yaw_kd = 0.000007                                          '0.0000096
'_roll_kd = 0.000007                                         '0.0000096
'_pitch_kd = 0.000007                                        '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte


'_bl(1) = rechts hinten  (US)
'_bl(2) = links hinten   (GUS)
'_bl(3) = rechts vorne   (GUS)
'_bl(4) = links vorne    (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms


'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################




'-------initialize the values

'_roll_kp = 0.4
'_roll_ki = 0.0015

T = 1.0
T_a = 0.75
F0 = 500
F = F0
W0_kp = 0.3
W0_ki = 0.001
W0_kd = 0.000005
Fp = 0.2
Fi = 0.0008
Fd = 0.00001
Log_step = 3
Max_log_cnt = 90
Max_tries = 30



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
   If Rc_on < 65000 Then Incr Rc_on
Else
   If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
   Goto Rc_test
End If




Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   '#####
   _bl(1) = 113
   _bl(4) = 113
   '#####
   Call Set_pwm()
   Call Simulated_annealing()


Loop


Sub Simulated_annealing()

   If State > 0 Then

      Incr Test_anz

      '-------randomize according to the current temperature--------
      Rp_i = Rnd(200) - 100
      Rp = Rp_i * 0.01
      Rp = Rp * T
      Ri_i = Rnd(200) - 100
      Ri = Ri_i * 0.01
      Ri = Ri * T
      Rd_i = Rnd(200) - 100
      Rd = Rd_i * 0.01
      Rd = Rd * T

      '-------generate new parameters------------------------------
      Wi_kp = Rp * Fp
      Wi_kp = Wi_kp + W0_kp
      Wi_ki = Ri * Fi
      Wi_ki = Wi_ki + W0_ki
      Wi_kd = Rd * Fd
      Wi_kd = Wi_kd + W0_kd

      '-------cut off parameters-----------------------------------
      If Wi_kp < 0 Then Wi_kp = 0
      If Wi_ki < 0 Then Wi_ki = 0

      '-------test current parameters------------------------------
      _roll_kp = Wi_kp
      _roll_ki = Wi_ki
      _roll_kd = Wi_kd

      '-------log data and get fitness-----------------------------
      Incr Log_cnt
      If Log_cnt < Max_log_cnt Then
         Ax(log_cnt) = _roll_err
         '####
         If Log_cnt > Log_step Then
            Atmp = Log_cnt - Log_step
         Else
            Atmp = 1
         End If
         Extmp = Ax(log_cnt) - Ax(atmp)
         Extmp = Extmp ^ 2
         Eytmp = Ay(log_cnt) - Ay(atmp)
         Eytmp = Eytmp ^ 2
         Eztmp = Az(log_cnt) - Az(atmp)
         Eztmp = Eztmp ^ 2
         Ex = Ex + Extmp
         Ey = Ey + Eytmp
         Ez = Ez + Eztmp
         '####
      Elseif Log_cnt = Max_log_cnt Then
         F = Ex + Ey
         F = F + Ez
         Ex = 0
         Ey = 0
         Ez = 0
         Log_cnt = 0
         Incr Tries
      End If

      '-------better then current solution?------------------------
      If F < F0 Then
         F0 = F
         W0_kp = Wi_kp
         W0_ki = Wi_ki
         W0_kd = Wi_kd
      End If

      '-------counter for max tries---------------------------------
      If Tries = Max_tries Then
         Toggle Portc.2
         T = T * T_a
         Tries = 0
      End If

   End If

End Sub


Sub Serial0charmatch()
   Clear Serialin
   If State = 0 Then
      'Print F
      Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
   End If
End Sub


Sub Set_pwm()
   If _bl(1) > 250 Then _bl(1) = 250
   If _bl(1) < 113 Then _bl(1) = 113
   If _bl(2) > 250 Then _bl(2) = 250
   If _bl(2) < 113 Then _bl(2) = 113
   If _bl(3) > 250 Then _bl(3) = 250
   If _bl(3) < 113 Then _bl(3) = 113
   If _bl(4) > 250 Then _bl(4) = 250
   If _bl(4) < 113 Then _bl(4) = 113
   Pwm2a = _bl(4)                                           'links vorne (US)
   Pwm2b = _bl(2)                                           'links hinten (GUS)
   Pwm1a = _bl(3)                                           'rechts vorne (GUS)
   Pwm1b = _bl(1)                                           'rechts hinten (US)
End Sub


Sub Failsave()
  If Channel >= 11 Then
    If Failure < 255 Then
      Incr Failure
    End If
  End If
  If Channel < 11 Then
    If Failure > 0 Then
      Decr Failure
    End If
  End If
End Sub


Measure:
 If Channel > 0 And Channel < 9 Then
   Kanal(channel) = Timer0
 End If
 Timer0 = 6
 Incr Channel
Return


Pausedetect:
  If Channel <> 0 Then
    Newvalsreceived = 1
  End If
  Channel = 0
Return


Sub Filter_gyro_data()
 Call Read_wmp_data()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

 If Newvalsreceived = 1 Then
   Newvalsreceived = 0

   For I = 1 To Maxchannel
     Fkanal(i) = Fkanal(i) * 3
     Fkanal(i) = Fkanal(i) + Kanal(i)
     Shift Fkanal(i) , Right , 2
     Hempf(i) = Fkanal(i) + 17
     Lempf(i) = Fkanal(i) - 17
     If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
        Kanal(i) = Fkanal(i)
     End If
   Next I

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
     If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
     If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
   End If

   For I = 2 To Maxchannel
     If Kanal(i) >= 62 And Kanal(i) < 139 Then
       _sbl(i) = Kanal(i) - 100
     End If
   Next I

   If Kanal(5) < 80 Then State = 0
   If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
   If Kanal(5) >= 115 Then State = 2


   Setpoint_yaw = _sbl(_yawchannel) * 1.5

   If State < 2 Then
      Setpoint_roll = _sbl(_rollchannel) * 0.6
      Setpoint_pitch = _sbl(_pitchchannel) * 0.6
   Else
      Setpoint_roll = _sbl(_rollchannel) * 1.5
      Setpoint_pitch = _sbl(_pitchchannel) * 1.5
   End If


   If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
     Call Set_wmp_offset()
   End If

   Oldstate = State

 End If

End Sub


Sub Wmp_init()
   I2cstart
   I2cwbyte &HA6
   I2cwbyte &HFE
   I2cwbyte &H04
   I2cstop
End Sub


Sub Send_zero()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
End Sub


Sub Read_wmp_data()
   Gosub Send_zero
   I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
   Yaw1 = Wmplus_buffer(1)
   Roll1 = Wmplus_buffer(2)
   Pitch1 = Wmplus_buffer(3)
   Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
   Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
   Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
   Shift Yaw , Right , 4
   Shift Roll , Right , 4
   Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_wmp_data()
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
   _yawoffset_int = _yawoffset
   _rolloffset_int = _rolloffset
   _pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
 _yaw_err = Setpoint_yaw - _yawnow
 _yaw_kp_err = _yaw_err * _yaw_kp
 _yaw_ki_sum = _yaw_ki_sum + _yaw_err
 If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
 _yaw_ki_err = _yaw_ki_sum * _yaw_ki
 _yaw_kd_err = _yaw_err - _yaw_kd_old
 _yaw_kd_old = _yaw_err
 _yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
 _roll_err = Setpoint_roll - _rollnow
 _roll_kp_err = _roll_err * _roll_kp
 _roll_ki_sum = _roll_ki_sum + _roll_err
 If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
 _roll_ki_err = _roll_ki_sum * _roll_ki
 _roll_kd_err = _roll_err - _roll_kd_old
 _roll_kd_old = _roll_err
 _roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
 _pitch_err = Setpoint_pitch - _pitchnow
 _pitch_kp_err = _pitch_err * _pitch_kp
 _pitch_ki_sum = _pitch_ki_sum + _pitch_err
 If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
 _pitch_ki_err = _pitch_ki_sum * _pitch_ki
 _pitch_kd_err = _pitch_err - _pitch_kd_old
 _pitch_kd_old = _pitch_err
 _pitch_kd_err = _pitch_kd_err * _pitch_kd


 _yaw_pid = _yaw_kp_err + _yaw_ki_err
 _yaw_pid = _yaw_pid + _yaw_kd_err
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_err
 _roll_pid = _roll_pid + _roll_kd_err
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_err
 _pitch_pid = _pitch_pid + _pitch_kd_err
 _pitch_pid_int = _pitch_pid

 If _yaw_pid_int < -68 Then _yaw_pid_int = -68
 If _yaw_pid_int > 68 Then _yaw_pid_int = 68
 If _roll_pid_int < -68 Then _roll_pid_int = -68
 If _roll_pid_int > 68 Then _roll_pid_int = 68
 If _pitch_pid_int < -68 Then _pitch_pid_int = -68
 If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

  '_bl(1) = rechts hinten  (US)
  '_bl(2) = links hinten   (GUS)
  '_bl(3) = rechts vorne   (GUS)
  '_bl(4) = links vorne    (US)

 _bl(1) = 182 + _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = _bl(1)

 If State <> 0 And Failure < 20 Then

   _bl(1) = _bl(1) + Minthrottle
   _bl(2) = _bl(2) + Minthrottle
   _bl(3) = _bl(3) + Minthrottle
   _bl(4) = _bl(4) + Minthrottle

   _bl(1) = _bl(1) - _pitch_pid_int
   _bl(4) = _bl(4) + _pitch_pid_int

   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int

   _bl(1) = _bl(1) - _yaw_pid_int
   _bl(2) = _bl(2) + _yaw_pid_int
   _bl(3) = _bl(3) + _yaw_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int

 Else

   _bl(1) = 113
   _bl(2) = 113
   _bl(3) = 113
   _bl(4) = 113

   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err = 0
   _pitch_ki_err = 0
   _yaw_pid_int = 0
   _roll_pid_int = 0
   _pitch_pid_int = 0
   _yaw_pid = 0
   _roll_pid = 0
   _pitch_pid = 0
   Yawstickvel = 0
   Rollstickvel = 0
   Pitchstickvel = 0
   Yawstickold = 0
   Rollstickold = 0
   Pitchstickold = 0

 End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Im Unterschied zu meiner vorherigen Version ist hier die Fitness-Funtkion anders aufgebaut. Es wird nicht, wie vorher, die Fitness der geloggten Daten auf einmal bestimmt, sondern es wird nach jedem Datenloggen wieder ein Teil der Fitness bestimmt. Das hat den Vorteil, dass die Rechenzeit für die Fitness-Funktion gleichmäßiger verteilt ist und nicht mehr auf einen kurzen Zeitpunkt fixiert. Somit kann die Regelung besser funktionieren.
Das einzige, was jetzt noch fehlt, ist die "kontrollierte unkontrollierte" Abweichung im System, z.b. eine Windböe in der Software nachgebildet ^^
Das möchte ich über die Variable Log_cnt realisieren: Wenn diese in dem Bereich von x1 bis x2 ist, wird eine RC-Eingabe simuliert (z.b. Vollausschlag links)... Hier kann ich dann versch. beliebige Muster einprogrammieren, evtl. sogar über eine GUI (wird sicherlich noch folgen).
Habe mir jetzt auch zwei Bücher bestellt, welche das Thema naturanaloge Algorithmen behandeln (also auch Simulated Annealing, genet. Algos, usw...). Vielleicht finde ich ja darin dann noch ein paar Tipps zur besseren Umsetzung... Es gäbe da noch einiges, z.b. die Möglichkeit, einen Kandidaten mit schlechterer Fitness trotzdem zu übernehmen, um lokale Minima zu vermeiden, usw... Das kommt aber erst, wenn der Grundalgo mal läuft. Aber so wies aussieht, läuft er schon
Wenn Interresse besteht, kann ich ja mal ein Video von meinem 5 Minuten Teststand reinstellen...

[Che Guevara]

Hallo,

nun bin ich wieder etwas weiter, nachdem ich die Fitness-Funktion debuggt habe. Jetzt kommen richtige Werte raus, vorher nicht! Außerdem habe ich jetzt noch eine Art Startup Prozedur eingebaut, d.h. erst schwingt sich der Quadro ein und dann wird die Fitness von den Startparametern bestimmt.
Hier der Code:

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400


Declare Sub Serial0charmatch()

Config Serialin = Buffered , Size = 30 , Bytematch = 13


Declare Sub Simulated_annealing()



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Long
Dim F0 As Long

'------evtl. als long!!!------------------------------
Dim Extmp As Long
Dim Eytmp As Long
Dim Eztmp As Long
Dim Ax(50) As Integer                                       'Integer
Dim Ay(50) As Integer                                       'Integer
Dim Az(50) As Integer                                       'Integer
Dim Ex As Long
Dim Ey As Long
Dim Ez As Long

Dim Atmp As Word
Dim A As Word

Dim Log_step As Byte
Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word

Dim Test_anz As Long


Dim X As Integer



'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################
Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 50                                      '30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.4                                               '0.32
_roll_kp = 0.4                                              '0.32
_pitch_kp = 0.4                                             '0.32

_yaw_ki = 0.0008                                            '0.0008
_roll_ki = 0.0015                                           '0.0018
_pitch_ki = 0.0015                                          '0.0018

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0

'_yaw_ki = 0.0001                                            '0.000135
'_roll_ki = 0.0001                                           '0.000135
'_pitch_ki = 0.0001                                          '0.000135

'_yaw_kd = 0.000007                                          '0.0000096
'_roll_kd = 0.000007                                         '0.0000096
'_pitch_kd = 0.000007                                        '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte

Dim New_parameters As Bit

Dim Sim_an_oldstate As Byte


'_bl(1) = rechts hinten  (US)
'_bl(2) = links hinten   (GUS)
'_bl(3) = rechts vorne   (GUS)
'_bl(4) = links vorne    (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms


'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################




'-------initialize the values

'_roll_kp = 0.4
'_roll_ki = 0.0015

T = 1.0
T_a = 0.98                                                  '0.8
F0 = 99999999
F = F0
W0_kp = 0.4
W0_ki = 0.0015                                              '0.001
W0_kd = 0.0                                                 '0.000005
Fp = 0.2
Fi = 0.001                                                  '0.0008
Fd = 0.00001                                                '0.00001
Log_step = 5
Max_log_cnt = 80                                            '50
Max_tries = 50                                              '25
New_parameters = 1


Dim Einschwingen As Word
Einschwingen = 0


Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
   If Rc_on < 65000 Then Incr Rc_on
Else
   If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
   Goto Rc_test
End If




Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   '#####
   _bl(1) = 113
   _bl(4) = 113
   '#####
   Call Set_pwm()
   If State > 0 Then
      Incr Einschwingen
   End If


Loop Until Einschwingen = 3000

Portc.2 = 1





Do


   Call Filter_rx_data()
   Call Filter_gyro_data()


   If State > 0 Then

      _roll_kp = W0_kp
      _roll_ki = W0_ki
      _roll_kd = W0_kd

      '-------log data and get fitness-----------------------------
      Incr Log_cnt
      If Log_cnt < Max_log_cnt Then
         Ax(log_cnt) = _roll_err                            '_rollnow
         If Log_cnt > Log_step Then
            Atmp = Log_cnt - Log_step
            Extmp = Ax(log_cnt) - Ax(atmp)
            Extmp = Extmp * Extmp
            Ex = Ex + Extmp
            If Log_cnt >= 10 And Log_cnt < 20 Then
               Setpoint_roll = 20
            Elseif Log_cnt >= 40 And Log_cnt < 50 Then
               Setpoint_roll = -20
            End If
         End If
      Elseif Log_cnt = Max_log_cnt Then
         F = Ex + Ey
         F = F + Ez
         F0 = F
         Print "Fitness: " ; F
         Ex = 0
         Ey = 0
         Ez = 0
         Log_cnt = 0
         Incr Tries
         New_parameters = 1
         Incr Test_anz
      End If

   End If


   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   '#####
   _bl(1) = 113
   _bl(4) = 113
   '#####
   Call Set_pwm()


Loop Until F <> 99999999





Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Simulated_annealing()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   '#####
   _bl(1) = 113
   _bl(4) = 113
   '#####
   Call Set_pwm()


Loop


Sub Simulated_annealing()

   If State > 0 Then

      '-------new parameters needed?--------------------------------
      If New_parameters = 1 Then

         New_parameters = 0

         '-------randomize according to the current temperature--------
         Rp_i = Rnd(200) - 100
         Rp = Rp_i * 0.01
         Rp = Rp * T
         Ri_i = Rnd(200) - 100
         Ri = Ri_i * 0.01
         Ri = Ri * T
         Rd_i = Rnd(200) - 100
         Rd = Rd_i * 0.01
         Rd = Rd * T

         '-------generate new parameters------------------------------
         Wi_kp = Rp * Fp
         Wi_kp = Wi_kp + W0_kp
         Wi_ki = Ri * Fi
         Wi_ki = Wi_ki + W0_ki
         Wi_kd = Rd * Fd
         Wi_kd = Wi_kd + W0_kd

         '-------cut off parameters-----------------------------------
         If Wi_kp < 0.25 Then Wi_kp = 0.25
         If Wi_kp > 0.5 Then Wi_kp = 0.5
         If Wi_ki < 0.0005 Then Wi_ki = 0.0005
         If Wi_ki > 0.003 Then Wi_ki = 0.003

         '-------test current parameters------------------------------
         _roll_kp = Wi_kp
         _roll_ki = Wi_ki
         _roll_kd = Wi_kd

      End If

      '-------log data and get fitness-----------------------------
      Incr Log_cnt
      If Log_cnt < Max_log_cnt Then
         Ax(log_cnt) = _roll_err                            '_rollnow
         If Log_cnt > Log_step Then
            Atmp = Log_cnt - Log_step
            Extmp = Ax(log_cnt) - Ax(atmp)
            Extmp = Extmp * Extmp
            Ex = Ex + Extmp
            If Log_cnt >= 10 And Log_cnt < 20 Then
               Setpoint_roll = 20
            Elseif Log_cnt >= 40 And Log_cnt < 50 Then
               Setpoint_roll = -20
            End If
         End If
      Elseif Log_cnt = Max_log_cnt Then
         F = Ex + Ey
         F = F + Ez
         'Print "Fitness: " ; F
         Ex = 0
         Ey = 0
         Ez = 0
         Log_cnt = 0
         Incr Tries
         New_parameters = 1
         Incr Test_anz
      End If

      '-------better then current solution?------------------------
      If F < F0 Then
         F0 = F
         W0_kp = Wi_kp
         W0_ki = Wi_ki
         W0_kd = Wi_kd
         Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
      End If

      '-------counter for max tries---------------------------------
      If Tries = Max_tries Then
         Toggle Portc.2
         T = T * T_a
         Tries = 0
      End If

   End If

End Sub


Sub Serial0charmatch()
   Clear Serialin
   If State = 0 Then
      Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
   End If
End Sub


Sub Set_pwm()
   If _bl(1) > 250 Then _bl(1) = 250
   If _bl(1) < 113 Then _bl(1) = 113
   If _bl(2) > 250 Then _bl(2) = 250
   If _bl(2) < 113 Then _bl(2) = 113
   If _bl(3) > 250 Then _bl(3) = 250
   If _bl(3) < 113 Then _bl(3) = 113
   If _bl(4) > 250 Then _bl(4) = 250
   If _bl(4) < 113 Then _bl(4) = 113
   Pwm2a = _bl(4)                                           'links vorne (US)
   Pwm2b = _bl(2)                                           'links hinten (GUS)
   Pwm1a = _bl(3)                                           'rechts vorne (GUS)
   Pwm1b = _bl(1)                                           'rechts hinten (US)
End Sub


Sub Failsave()
  If Channel >= 11 Then
    If Failure < 255 Then
      Incr Failure
    End If
  End If
  If Channel < 11 Then
    If Failure > 0 Then
      Decr Failure
    End If
  End If
End Sub


Measure:
 If Channel > 0 And Channel < 9 Then
   Kanal(channel) = Timer0
 End If
 Timer0 = 6
 Incr Channel
Return


Pausedetect:
  If Channel <> 0 Then
    Newvalsreceived = 1
  End If
  Channel = 0
Return


Sub Filter_gyro_data()
 Call Read_wmp_data()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

 If Newvalsreceived = 1 Then
   Newvalsreceived = 0

   For I = 1 To Maxchannel
     Fkanal(i) = Fkanal(i) * 3
     Fkanal(i) = Fkanal(i) + Kanal(i)
     Shift Fkanal(i) , Right , 2
     Hempf(i) = Fkanal(i) + 17
     Lempf(i) = Fkanal(i) - 17
     If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
        Kanal(i) = Fkanal(i)
     End If
   Next I

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
     If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
     If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
   End If

   For I = 2 To Maxchannel
     If Kanal(i) >= 62 And Kanal(i) < 139 Then
       _sbl(i) = Kanal(i) - 100
     End If
   Next I

   If Kanal(5) < 80 Then State = 0
   If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
   If Kanal(5) >= 115 Then State = 2


   Setpoint_yaw = _sbl(_yawchannel) * 1.5

   If State < 2 Then
      Setpoint_roll = _sbl(_rollchannel) * 0.6
      Setpoint_pitch = _sbl(_pitchchannel) * 0.6
   Else
      Setpoint_roll = _sbl(_rollchannel) * 1.5
      Setpoint_pitch = _sbl(_pitchchannel) * 1.5
   End If


   If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
     Call Set_wmp_offset()
   End If

   Oldstate = State

 End If

End Sub


Sub Wmp_init()
   I2cstart
   I2cwbyte &HA6
   I2cwbyte &HFE
   I2cwbyte &H04
   I2cstop
End Sub


Sub Send_zero()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
End Sub


Sub Read_wmp_data()
   Gosub Send_zero
   I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
   Yaw1 = Wmplus_buffer(1)
   Roll1 = Wmplus_buffer(2)
   Pitch1 = Wmplus_buffer(3)
   Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
   Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
   Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
   Shift Yaw , Right , 4
   Shift Roll , Right , 4
   Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_wmp_data()
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
   _yawoffset_int = _yawoffset
   _rolloffset_int = _rolloffset
   _pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
 _yaw_err = Setpoint_yaw - _yawnow
 _yaw_kp_err = _yaw_err * _yaw_kp
 _yaw_ki_sum = _yaw_ki_sum + _yaw_err
 If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
 _yaw_ki_err = _yaw_ki_sum * _yaw_ki
 _yaw_kd_err = _yaw_err - _yaw_kd_old
 _yaw_kd_old = _yaw_err
 _yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
 _roll_err = Setpoint_roll - _rollnow
 _roll_kp_err = _roll_err * _roll_kp
 _roll_ki_sum = _roll_ki_sum + _roll_err
 If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
 _roll_ki_err = _roll_ki_sum * _roll_ki
 _roll_kd_err = _roll_err - _roll_kd_old
 _roll_kd_old = _roll_err
 _roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
 _pitch_err = Setpoint_pitch - _pitchnow
 _pitch_kp_err = _pitch_err * _pitch_kp
 _pitch_ki_sum = _pitch_ki_sum + _pitch_err
 If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
 _pitch_ki_err = _pitch_ki_sum * _pitch_ki
 _pitch_kd_err = _pitch_err - _pitch_kd_old
 _pitch_kd_old = _pitch_err
 _pitch_kd_err = _pitch_kd_err * _pitch_kd


 _yaw_pid = _yaw_kp_err + _yaw_ki_err
 _yaw_pid = _yaw_pid + _yaw_kd_err
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_err
 _roll_pid = _roll_pid + _roll_kd_err
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_err
 _pitch_pid = _pitch_pid + _pitch_kd_err
 _pitch_pid_int = _pitch_pid

 If _yaw_pid_int < -68 Then _yaw_pid_int = -68
 If _yaw_pid_int > 68 Then _yaw_pid_int = 68
 If _roll_pid_int < -68 Then _roll_pid_int = -68
 If _roll_pid_int > 68 Then _roll_pid_int = 68
 If _pitch_pid_int < -68 Then _pitch_pid_int = -68
 If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

  '_bl(1) = rechts hinten  (US)
  '_bl(2) = links hinten   (GUS)
  '_bl(3) = rechts vorne   (GUS)
  '_bl(4) = links vorne    (US)

 _bl(1) = 182 + _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = _bl(1)

 If State <> 0 And Failure < 20 Then

   _bl(1) = _bl(1) + Minthrottle
   _bl(2) = _bl(2) + Minthrottle
   _bl(3) = _bl(3) + Minthrottle
   _bl(4) = _bl(4) + Minthrottle

   _bl(1) = _bl(1) - _pitch_pid_int
   _bl(4) = _bl(4) + _pitch_pid_int

   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int

   _bl(1) = _bl(1) - _yaw_pid_int
   _bl(2) = _bl(2) + _yaw_pid_int
   _bl(3) = _bl(3) + _yaw_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int

 Else

   _bl(1) = 113
   _bl(2) = 113
   _bl(3) = 113
   _bl(4) = 113

   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err = 0
   _pitch_ki_err = 0
   _yaw_pid_int = 0
   _roll_pid_int = 0
   _pitch_pid_int = 0
   _yaw_pid = 0
   _roll_pid = 0
   _pitch_pid = 0
   Yawstickvel = 0
   Rollstickvel = 0
   Pitchstickvel = 0
   Yawstickold = 0
   Rollstickold = 0
   Pitchstickold = 0

 End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Jetzt muss ich nur noch die Simulated-Annealing Parameter richtig einstellen xD Aber vom Prinzip her funktionierts.
Evtl. werd ich das ganze morgen mal, mit kleiner Temperatur, im richtigen Flug (also ohne Teststand) probieren... Mal sehen.

Gruß
Chris

EDIT:
Um das ganze besser testen zu können, verwende ich nicht die Gyro-werte für die Fitness-Berechnung, sondern den Fehler aus Gyro-wert minus RC-wert. Somit kann auch mit einbezogen werden, wie gut die Parameter dem Piloten folgen. Was haltet ihr davon?