Hallo,
gibt es schon neuigkeiten?
Gruß, Bammel
Druckbare Version
Hallo,
gibt es schon neuigkeiten?
Gruß, Bammel
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
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:
oder 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
nach BASIC übersetzt?Code:if fcandidate < f0 then
temperatur = temperatur * t_a
endif
w0_kp = wcandidate_kp
w0_ki = wcandidate_ki
w0_kd = wcandidate_kd
f0 = fcandidate
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:
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)?!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
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
Hinter der IF Abfrage ist keine {-Klammer weil nur ein Kommando ("T*=a") steht.Zitat:
Zitat von Che Guevara
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
Tip: Ich kenne mich zwar mit Optimierungsmethoden wenig aus, weiß aber, dass MATLAB eine Menge Algorithmen hierfür bereithält.
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:
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.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
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...
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:
Jetzt muss ich nur noch die Simulated-Annealing Parameter richtig einstellen xD Aber vom Prinzip her funktionierts.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
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?
Hallo,
das sieht sehr interessant aus. hab mal nen genalg am standartbeispiel handlungsreisende in java gemacht. das ganze für einen pid regler ist ne feine sache. gibts schon ein video oder ist es angedacht?
Hallo,
ja, das beispiel kenne ich auch, aber ehrlich gesagt finde ich das komplizierter als für die PID-Regelung...
Ein Video wird folgen, sobald die Ergebnisse einigermassen brauchbar sind. Momentan kämpfe ich mit den richtigen Parametern fürs Simulated Annealing. Da ich jetzt aber auch 2 Bücher zum Thema habe, werde ich auch mal versuchen, einen genet. Algo dafür zu schreiben, weil ich glaube, dass sich dieser besser eignet.... mal sehen.
Gruß
Chris
Hallo,
nachdem ich nun längere Zeit nichts mehr gemacht habe, melde ich mich mal wieder. Gestern Abend habe ich auf die schnelle mal eine Quick&Dirty Umsetzung eines SA in vb.net geschrieben. Das Programm funktioniert m.E.n. sehr gut, man kann gut erkennen, wie sich die Parameter aufs Ergebnis auswirken. Wers mal testen möchte, das Prog ist im Anhang. Einfach Ta und Y eingeben und auf Start drücken (bitte für Fließkommaeingaben nur den . verwenden). Wie gesagt, ist Quick&Dirty, aber es stellt für mich einen großen Ansporn dar, weiter zu machen. Heute werde ich mich mal an einem GA in vb.net versuchen.
Gruß
Chris