Tip: Ich kenne mich zwar mit Optimierungsmethoden wenig aus, weiß aber, dass MATLAB eine Menge Algorithmen hierfür bereithält.
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...
Geändert von Che Guevara (14.11.2011 um 21:46 Uhr)
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?
Geändert von Che Guevara (16.11.2011 um 17:26 Uhr)
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
Hallo,
gestern habe ich die ersten erfolgreichen Tests mit meinem GA durchgeführt. Das Programm ist wieder Quick&Dirty, funktioniert aber. Hier zeigen sich auch schon die großen Unterschiede zw. SA und GA. Beim GA passiert es des öfteren, dass er nie eine optimale Lösung findet und in irgendeinem lokalen Minimum hängen bleibt. Außerdem benötigt man eine relativ große Population, um mehr oder minder sicher und schnell das optimale Ergebnis zu erreichen. Als guten Wert hat sich jetzt bei mir der Wert 500 (für m) herauskristallisiert. Allerdings muss ich dazu sagen, dass bis jetzt keine Mutation integriert ist (kommt aber noch) und die Rekombination ist eine einfache Mittelwertbildung von den 2 Werten. Auch das Heiratsschema ist nicht optimal, allerdings habe ich bis jetzt noch keinerlei Quellen gefunden, wie man ein einigermaßen gutes Heiratsschema implementiert... Wenn jemand da einen Tipp hat, immer her damit
Das Programm ist wieder im Anhang, setzt jedoch, genauso wie das SA das .net Framework vorraus.
Gruß
Chris
EDIT:
Gerade eben habe ich das Heiratsschema verändert. Jetzt werden zufällig einzelne Individuen miteinander rekombiniert, das brachte eine viel kürzere Laufzeit bei nur m = 50. Das neue Programm ist im Anhang.
Geändert von Che Guevara (05.02.2012 um 23:36 Uhr)
Berechtigungen
Zitieren
Lesezeichen