Hallo,
mein Tricopter ist nun soweit fertig, dass ich ihn denke ich auch vorzeigen kannAllerdings gibt es noch ein paar Sachen, die mich stören! Das erste Problem ist folgendes:
Wenn ich in eine Richtung steuere, fliegt der Tri auch dorthin, allerdings dauert es ziemlich lange, bis er sich wieder in die Waagrechte Position begibt, nachdem ich den Knüppel loslasse (z.b. roll). Eigentlich sollte man das ja mit dem D-Anteil des PID-Reglers schneller machen können, aber mein D-Anteil steht momentan auf 0, da der Tri ansonsten zu Schwingen beginnt. Wie kann ich es nun erreichen, dass er schneller wieder die "Ruheposition" einnimmt? Den I-Anteil vergrößern klappt nicht, da er sonst auch schwingt! Hier ist auch das nächste Problem: Die Dimensionierung des Anti-Windup für den I-Anteil. Ich habe momentan keinerlei Anhaltspunkte über desen Größenordnung und ehrlich gesagt kommt Try and Error für mich in diesem Fall nicht in Frage, da ich das schon einmal gemacht habe und dabei ist mir der halbe Tri auseinandergebrochen, weil der max. Wert des I-Anteils zu klein war.... Aber um das geht es hier jetzt nicht (momentan).
Wäre es möglich, die Stickposition (also die Werte der Kanäle) mit dem letzten Wert zu differenzieren und somit dann gegenzusteuern? Ich denke, das sollte funktionieren, aber bevor ich irgendetwas schrotte, dachte ich, ich frage mal hier nach?!
Vielen Dank & GrußCode:$regfile = "m328pdef.dat" $crystal = 16000000 $framesize = 200 $hwstack = 200 $swstack = 200 $baud = 1000000 '115200 Open "COMC.3:19200,8,N,1" For Input As #1 Open "COMC.2:19200,8,N,1" For Output As #2 Declare Sub Init_system() Declare Sub Filter_gyro_data() Declare Sub Filter_rx_data() Declare Sub Mixer() Declare Sub Send_data() Declare Sub Pid_regulator() Declare Sub Wmp_init() Declare Sub Send_zero() Declare Sub Read_wmp_data() Declare Sub Set_wmp_offset() $lib "I2C_TWI.LBX" Config Scl = Portc.5 Config Sda = Portc.4 Config Twi = 100000 I2cinit Config Timer0 = Timer , Prescale = 256 On Timer0 Pausedetect Enable Timer0 Config Int0 = Falling On Int0 Measure Enable Int0 Config Pind.2 = Input Portd.2 = 0 Config Portd.4 = Output Led Alias Portd.4 Led = 0 '#################################### '###########KONSTANTEN############### '#################################### Const _maxchannel = 8 Const Start_byte = 127 Const _throttlechannel = 1 Const _rollchannel = 2 Const _pitchchannel = 3 Const _yawchannel = 4 Const _statechannel = 5 Const _datachannel = 6 Const _bl4offset = 0 Const _max_d_state = 3 '################################### '################################### '################################### '################################### '###########PID-PARAMETER########### '################################### 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 '#########PID-Werte############ '( P : 0.675000008:1.049999949 I : 0.00139999:0.004399952 D: ') '############################## _yaw_kp = 1.5 _roll_kp = 0.675000008 _pitch_kp = 1.049999949 _yaw_ki = 0 _roll_ki = 0.00139999 _pitch_ki = 0.004399952 _yaw_kd = 0 _roll_kd = 0 _pitch_kd = 0.00079995 '################################### '################################### '################################### '################################### '###############TMP################# '################################### Dim _btm222_counter As Byte Dim I As Byte Dim Newvalsreceived As Bit '################################### '################################### '################################### '################################### '#########RC-EMPFÄNGER############## '################################### Dim Bufferbyte As Byte Dim Kanal(_maxchannel) As Word Dim _lp_bandwidth As Byte Dim _hkanal(_maxchannel) As Word Dim _lkanal(_maxchannel) As Word Dim _kanal_filter(_maxchannel) As Word Dim Channel As Byte Dim _bl(4) As Word Dim _crc As Word Dim _sbl(_maxchannel) As Integer Dim _sbl_filter(_maxchannel) As Integer '################################### '################################### '################################### '################################### '###########I2C-Inputs############## '################################### Dim Wmplus_buffer(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 _d_state As Byte 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_single As Single Dim _roll_kd_err_single As Single Dim _pitch_kd_err_single As Single Dim _yaw_kd_err(_max_d_state) As Single Dim _roll_kd_err(_max_d_state) As Single Dim _pitch_kd_err(_max_d_state) As Single Dim _yaw_kd_old(_max_d_state) As Single Dim _roll_kd_old(_max_d_state) As Single Dim _pitch_kd_old(_max_d_state) 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 _yawstickvel As Integer Dim _rollstickvel As Integer Dim _pitchstickvel As Integer Dim _sbl_old(_maxchannel) As Integer Dim _x1 As Single Dim _x2 As Single Wait 2 Call Init_system() Enable Interrupts Do Call Filter_rx_data() '580 Call Filter_gyro_data() '1900 Call Pid_regulator() '700 Call Mixer() '210 Call Send_data() '470 Loop 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 If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100 _sbl(_throttlechannel) = _sbl(_throttlechannel) * 30 If _sbl(_throttlechannel) > 900 Then _sbl(_throttlechannel) = 900 If _sbl(_throttlechannel) < -1000 Then _sbl(_throttlechannel) = -1000 End If For I = 2 To _maxchannel If Kanal(i) >= 60 And Kanal(i) <= 140 Then _sbl(i) = Kanal(i) - 100 If _sbl(i) > -2 And _sbl(i) < 2 Then _sbl(i) = 0 _sbl(i) = _sbl(i) * 25 End If Next I _sbl_filter(_throttlechannel) = _sbl_filter(_throttlechannel) + _sbl(_throttlechannel) _sbl_filter(_throttlechannel) = _sbl_filter(_throttlechannel) / 2 _sbl(_throttlechannel) = _sbl_filter(_throttlechannel) _sbl_filter(_rollchannel) = _sbl_filter(_rollchannel) + _sbl(_rollchannel) _sbl_filter(_rollchannel) = _sbl_filter(_rollchannel) / 2 _sbl(_rollchannel) = _sbl_filter(_rollchannel) _sbl_filter(_pitchchannel) = _sbl_filter(_pitchchannel) + _sbl(_pitchchannel) _sbl_filter(_pitchchannel) = _sbl_filter(_pitchchannel) / 2 _sbl(_pitchchannel) = _sbl_filter(_pitchchannel) _sbl_filter(_yawchannel) = _sbl_filter(_yawchannel) + _sbl(_yawchannel) _sbl_filter(_yawchannel) = _sbl_filter(_yawchannel) / 2 _sbl(_yawchannel) = _sbl_filter(_yawchannel) _sbl_filter(_statechannel) = _sbl_filter(_statechannel) * 19 _sbl_filter(_statechannel) = _sbl_filter(_statechannel) + _sbl(_statechannel) _sbl_filter(_statechannel) = _sbl_filter(_statechannel) / 20 _sbl(_statechannel) = _sbl_filter(_statechannel) _sbl_filter(_datachannel) = _sbl_filter(_datachannel) + _sbl(_datachannel) _sbl_filter(_datachannel) = _sbl_filter(_datachannel) / 2 _sbl(_datachannel) = _sbl_filter(_datachannel) If _sbl(_statechannel) < 200 Then _sbl(_rollchannel) = _sbl(_rollchannel) / 5 '5 _sbl(_pitchchannel) = _sbl(_pitchchannel) / 5 Elseif _sbl(_statechannel) > 200 Then _sbl(_rollchannel) = _sbl(_rollchannel) / 2 '3 _sbl(_pitchchannel) = _sbl(_pitchchannel) / 3 End If _sbl(_yawchannel) = _sbl(_yawchannel) / 2 _yawstickvel = _sbl(_yawchannel) - _sbl_old(_yawchannel) _sbl_old(_yawchannel) = _sbl(_yawchannel) _rollstickvel = _sbl(_rollchannel) - _sbl_old(_rollchannel) _sbl_old(_rollchannel) = _sbl(_rollchannel) _pitchstickvel = _sbl(_pitchchannel) - _sbl_old(_pitchchannel) _sbl_old(_pitchchannel) = _sbl(_pitchchannel) '( _kanal_filter(7) = _kanal_filter(7) * 3 _kanal_filter(7) = _kanal_filter(7) + Kanal(7) Shift _kanal_filter(7) , Right , 2 Kanal(7) = _kanal_filter(7) _kanal_filter(8) = _kanal_filter(8) * 3 _kanal_filter(8) = _kanal_filter(8) + Kanal(8) Shift _kanal_filter(8) , Right , 2 Kanal(8) = _kanal_filter(8) ') '( '#########P-GAIN########### _x1 = Kanal(7) - 100 _x1 = _x1 * 0.075 _x2 = Kanal(8) - 100 _x2 = _x2 * 0.075 _roll_kp = 0 + _x1 _pitch_kp = 0 + _x2 '#########I-GAIN########### _x1 = Kanal(7) - 100 _x1 = _x1 * 0.0002 _x2 = Kanal(8) - 100 _x2 = _x2 * 0.0002 _roll_ki = 0.0 + _x1 _pitch_ki = 0.0 + _x2 '#########D-GAIN########### _x1 = Kanal(7) - 100 _x1 = _x1 * 0.0002 _x2 = Kanal(8) - 100 _x2 = _x2 * 0.0002 _roll_kd = 0.0 + _x1 _pitch_kd = 0.0 + _x2 ') If _sbl(_pitchchannel) > 170 And _sbl(_yawchannel) > 200 And _sbl(_throttlechannel) < -950 And _sbl(_statechannel) < -200 Then Call Set_wmp_offset() Led = 0 If _sbl(_datachannel) > -200 Then Print #2 , "OFFSET neu berechnet!" End If 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 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 , 2 Shift Roll , Right , 2 Shift Pitch , Right , 2 End Sub Sub Set_wmp_offset() _yawoffset = 0 _rolloffset = 0 _pitchoffset = 0 For I = 1 To 100 Waitms 5 Call Read_wmp_data() _yawoffset = _yawoffset + Yaw _rolloffset = _rolloffset + Roll _pitchoffset = _pitchoffset + Pitch Toggle Led Next I Led = 0 _yawoffset = _yawoffset / 100 _rolloffset = _rolloffset / 100 _pitchoffset = _pitchoffset / 100 _yawoffset_int = _yawoffset _rolloffset_int = _rolloffset _pitchoffset_int = _pitchoffset End Sub Sub Pid_regulator() '##############FEHLER_BERECHNEN########## '( _yaw_err_int = _sbl(_yawchannel) + _yawstickvel _yaw_err_int = _yaw_err_int - _yawnow _yaw_err = _yaw_err_int _roll_err_int = _sbl(_rollchannel) + _rollstickvel _roll_err_int = _roll_err_int - _rollnow _roll_err = _roll_err_int _pitch_err_int = _sbl(_pitchchannel) + _pitchstickvel _pitch_err_int = _pitch_err_int - _rollnow _pitch_err = _pitch_err_int ') _yaw_err_int = _sbl(_yawchannel) - _yawnow _yaw_err = _yaw_err_int _roll_err_int = _sbl(_rollchannel) - _rollnow _roll_err = _roll_err_int _pitch_err_int = _sbl(_pitchchannel) - _pitchnow _pitch_err = _pitch_err_int '##############PROPORTIONAL############## _yaw_kp_err = _yaw_err * _yaw_kp _roll_kp_err = _roll_err * _roll_kp _pitch_kp_err = _pitch_err * _pitch_kp '##############INTEGRAL################## _yaw_ki_err = _yaw_err * _yaw_ki _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err _roll_ki_err = _roll_err * _roll_ki _roll_ki_sum = _roll_ki_sum + _roll_ki_err _pitch_ki_err = _pitch_err * _pitch_ki _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err '##############DIFFERENTIAL############# Incr _d_state If _d_state > _max_d_state Then _d_state = 1 _yaw_kd_err(_d_state) = _yaw_err * _yaw_kd _yaw_kd_err(_d_state) = _yaw_kd_err(_d_state) - _yaw_kd_old(_d_state) 'evtl. anders herum!!!!!!!!!! _yaw_kd_old(_d_state) = _yaw_kd_err(_d_state) _roll_kd_err(_d_state) = _roll_err * _roll_kd _roll_kd_err(_d_state) = _roll_kd_err(_d_state) - _roll_kd_old(_d_state) 'evtl. anders herum!!!!!!!!!! _roll_kd_old(_d_state) = _roll_kd_err(_d_state) _pitch_kd_err(_d_state) = _pitch_err * _pitch_kd _pitch_kd_err(_d_state) = _pitch_kd_err(_d_state) - _pitch_kd_old(_d_state) 'evtl. anders herum!!!!!!!!!! _pitch_kd_old(_d_state) = _pitch_kd_err(_d_state) _yaw_kd_err_single = _yaw_kd_err(_d_state) _roll_kd_err_single = _roll_kd_err(_d_state) _pitch_kd_err_single = _pitch_kd_err(_d_state) '##############AUFSUMMIEREN############## _yaw_pid = _yaw_kp_err + _yaw_ki_sum _yaw_pid = _yaw_pid + _yaw_kd_err_single _yaw_pid_int = _yaw_pid _roll_pid = _roll_kp_err + _roll_ki_sum _roll_pid = _roll_pid + _roll_kd_err_single _roll_pid_int = _roll_pid _pitch_pid = _pitch_kp_err + _pitch_ki_sum _pitch_pid = _pitch_pid + _pitch_kd_err_single _pitch_pid_int = _pitch_pid '###############WERTE_BEGRENZEN########## If _yaw_pid_int < -1000 Then _yaw_pid_int = -1000 If _yaw_pid_int > 1000 Then _yaw_pid_int = 1000 If _roll_pid_int < -1000 Then _roll_pid_int = -1000 If _roll_pid_int > 1000 Then _roll_pid_int = 1000 If _pitch_pid_int < -1000 Then _pitch_pid_int = -1000 If _pitch_pid_int > 1000 Then _pitch_pid_int = 1000 End Sub Sub Mixer() _bl(1) = 62667 - _sbl(_throttlechannel) _bl(2) = _bl(1) _bl(3) = _bl(1) _bl(4) = 62667 _bl(4) = _bl(4) + _bl4offset If _sbl(_statechannel) > -200 Then _bl(1) = _bl(1) + _pitch_pid_int _bl(2) = _bl(2) - _roll_pid_int _bl(3) = _bl(3) + _roll_pid_int _pitch_pid_int = _pitch_pid_int / 2 _bl(2) = _bl(2) - _pitch_pid_int _bl(3) = _bl(3) - _pitch_pid_int _bl(4) = _bl(4) - _yaw_pid_int Led = 1 Elseif _sbl(_statechannel) < -200 Then _bl(1) = 63800 _bl(2) = 63800 _bl(3) = 63800 For I = 0 To _max_d_state _yaw_kd_err(i) = 0 _yaw_kd_old(i) = 0 _roll_kd_err(i) = 0 _roll_kd_old(i) = 0 _pitch_kd_err(i) = 0 _pitch_kd_old(i) = 0 Next I _yaw_kp_err = 0 _yaw_ki_sum = 0 _yaw_kd_err_single = 0 _yaw_ki_err = 0 _roll_kp_err = 0 _roll_ki_sum = 0 _roll_kd_err_single = 0 _roll_ki_err = 0 _pitch_kp_err = 0 _pitch_ki_sum = 0 _pitch_kd_err_single = 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 Led = 0 End If End Sub Sub Send_data() _crc = Crc16(_bl(1) , 4) Printbin Start_byte Incr _btm222_counter If _btm222_counter = 20 Then _btm222_counter = 0 If _sbl(_datachannel) > -200 Then 'Print #2 , _roll_kp ; ":" ; _pitch_kp 'Print #2 , _roll_ki ; ":" ; _pitch_ki 'Print #2 , _roll_kd ; ":" ; _pitch_kd 'Print #2 , _sbl(_throttlechannel) ; ":" ; _sbl(_rollchannel) ; ":" ; _sbl(_pitchchannel) ; ":" ; _sbl(_yawchannel) ; ":" ; _sbl(_statechannel) ; ":" ; _sbl(_datachannel) End If End If Printbin _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc End Sub Sub Init_system() _sbl_filter(_throttlechannel) = -1000 _sbl_filter(_rollchannel) = 0 _sbl_filter(_pitchchannel) = 0 _sbl_filter(_yawchannel) = 0 _sbl_filter(_statechannel) = -600 _sbl_filter(_datachannel) = -600 Reset Newvalsreceived _yawnow = 0 _rollnow = 0 _pitchnow = 0 Call Wmp_init() Wait 1 For I = 1 To 50 Call Read_wmp_data() Next I Wait 1 Call Set_wmp_offset() For I = 1 To 5 Led = 1 Waitms 20 Led = 0 Waitms 100 Next I End Sub End
Chris
Zitieren
Lesezeichen