Genetischer Algorithmus für QuadroCopter

[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?