Genetischer Algorithmus für QuadroCopter

**Che Guevara** · 13.11.2011, 13:29

Hallo,

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

Code:

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

Wird das so:

Code:

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:

Code:

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:

Code:

$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.

Code:

$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