Che Guevara
27.07.2011, 17:00
Hallo,
mein Tricopter ist nun soweit fertig, dass ich ihn denke ich auch vorzeigen kann :) Allerdings 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?! :D
$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
Vielen Dank & Gruß
Chris
mein Tricopter ist nun soweit fertig, dass ich ihn denke ich auch vorzeigen kann :) Allerdings 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?! :D
$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
Vielen Dank & Gruß
Chris