Che Guevara
27.06.2011, 21:26
Hallo,
mein Tricopter funktioniert eigentlich ganz gut, jedoch wenn ich zu stark steuere, dann fängt er kurz an stark zu Taumeln! Wie kann ich dieses Taumeln unterdrücken? Mit dem D-Anteil? Allerdings weiß ich nicht, ob ich diesen positiv oder negativ einstellen soll? Habe schon beide Varianten gesehen! Kann mir jemand erklären, wo der Unterschied liegt? Habe leider nichts im Internet darüber gefunden..
Hier der Code:
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400
Open "COMC.2:19200,8,N,1" For Input As #1
Open "COMC.3:19200,8,N,1" For Output As #2
Declare Sub Failsave()
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 '0
Config Sda = Portc.4 '1
Config Twi = 100000
I2cinit
Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1
Config Int0 = Falling
On Int0 Measure
Enable Int0
Config Pind.2 = Input
Portd.2 = 0
Config Timer2 = Timer , Prescale = 64
Timer2 = 6
On Timer2 Send_info
Disable Timer2
Stop Timer2
'Start Timer2
'Enable Timer2
Dim _timer2_counter As Word
'####################################
'###########KONSTANTEN###############
'####################################
Const _maxchannel = 4
Const Start_byte = 127
Dim _yaw_kp As Integer
Dim _roll_kp As Integer
Dim _pitch_kp As Integer
Dim _yaw_ki As Integer
Dim _roll_ki As Integer
Dim _pitch_ki As Integer
Dim _yaw_kd As Integer
Dim _roll_kd As Integer
Dim _pitch_kd As Integer
_yaw_kp = 10 '10
_roll_kp = 4 '4
_pitch_kp = 8 '8
_yaw_ki = 5 '5
_roll_ki = 9 '9 'evtl. i groesser!!!
_pitch_ki = 13 '13
_yaw_kd = 0 '0
_roll_kd = -4 '-4
_pitch_kd = -4 '-4
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = 100
'###################################
'###################################
'###################################
'###################################
'#########FAILSAVE##################
'###################################
Dim _failsave As Word
Dim _motor_stop As Bit
'###################################
'###################################
'###################################
'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer
Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word
'###################################
'###################################
'###################################
'###################################
'###########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 _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer
Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer
Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer
Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer
Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer
Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer
Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer
'#################################
'#################################
'#################################
'#################################
'#########IIR-FILTER##############
'#################################
Dim _yaw_filter As Integer
Dim _yaw_filter_2 As Integer
Dim _roll_filter As Integer
Dim _roll_filter_2 As Integer
Dim _pitch_filter As Integer
Dim _pitch_filter_2 As Integer
Dim _sbl_filter(4) As Integer
Dim _sbl_filter_2(4) As Integer
'#################################
'#################################
'#################################
Wait 2
Call Init_system()
Enable Interrupts
Do
'Call Failsave
Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
'_motor_stop = 1
Call Mixer()
Call Send_data()
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()
For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I
'_sbl(2) = _sbl(2) * -1
'_sbl(3) = _sbl(3) * -1
'_sbl(4) = _sbl(4) * -1
_sbl(2) = _sbl(2) * 10 'PITCH
_sbl(2) = _sbl(2) / 110 '110
_sbl(3) = _sbl(3) * 10 'ROLL
_sbl(3) = _sbl(3) / 110 '130
_sbl(4) = _sbl(4) * 10 'YAW
_sbl(4) = _sbl(4) / 30 '30
'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)
'(
For I = 1 To 4
_sbl_filter(i) = _sbl_filter(i) * 6
_sbl_filter(i) = _sbl_filter(i) / 10
_sbl_filter_2(i) = _sbl(i) * 4
_sbl_filter_2(i) = _sbl_filter_2(i) / 10
_sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
_sbl(i) = _sbl_filter(i)
Next I
')
End Sub
Measure:
If Channel > 0 And Channel < 5 Then
Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return
Pausedetect:
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
'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)
'(
_yaw_filter = _yaw_filter * 4
_yaw_filter = _yaw_filter / 10
_yaw_filter_2 = Yaw * 6
_yaw_filter_2 = _yaw_filter_2 / 10
_yaw_filter = _yaw_filter + _yaw_filter_2
Yaw = _yaw_filter
_roll_filter = _roll_filter * 4
_roll_filter = _roll_filter / 10
_roll_filter_2 = Roll * 6
_roll_filter_2 = _roll_filter_2 / 10
_roll_filter = _roll_filter + _roll_filter_2
Roll = _roll_filter
_pitch_filter = _pitch_filter * 4
_pitch_filter = _pitch_filter / 10
_pitch_filter_2 = Pitch * 6
_pitch_filter_2 = _pitch_filter_2 / 10
_pitch_filter = _pitch_filter + _pitch_filter_2
Pitch = _pitch_filter
')
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
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()
'##############FEHLER_BERECHNEN##########
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow
_roll_err = _sbl(3) / 1
_roll_err = _roll_err - _rollnow
_pitch_err = _sbl(2) / 1
_pitch_err = _pitch_err - _pitchnow
'##############PROPORTIONAL##############
_yaw_kp_err = _yaw_err * _yaw_kp
_yaw_kp_err = _yaw_kp_err / 20 '20
_roll_kp_err = _roll_err * _roll_kp
_roll_kp_err = _roll_kp_err / 20
_pitch_kp_err = _pitch_err * _pitch_kp
_pitch_kp_err = _pitch_kp_err / 20
'##############INTEGRAL##################
_yaw_ki_err = _yaw_err * _yaw_ki
_yaw_ki_err = _yaw_ki_err / 2000 '2000
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_roll_ki_err = _roll_err * _roll_ki
_roll_ki_err = _roll_ki_err / 2000
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_pitch_ki_err = _pitch_err * _pitch_ki
_pitch_ki_err = _pitch_ki_err / 2000
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
'##############DIFFERENNTIAL#############
_yaw_kd_err = _yaw_err * _yaw_kd
_yaw_kd_err = _yaw_kd_err / 4000 '4000
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_err
_roll_kd_err = _roll_err * _roll_kd
_roll_kd_err = _roll_kd_err / 4000
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_err
_pitch_kd_err = _pitch_err * _pitch_kd
_pitch_kd_err = _pitch_kd_err / 4000
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_err
'##############AUFSUMMIEREN##############
_yaw_pid = _yaw_kp_err + _yaw_ki_sum
_yaw_pid = _yaw_pid + _yaw_kd_err
_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err
_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err
'###############WERTE_BEGRENZEN##########
If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub
Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset
If _sbl(1) > -920 Then
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_pitch_pid = _pitch_pid / 2 '############
_bl(2) = _bl(2) - _pitch_pid '############
_bl(3) = _bl(3) - _pitch_pid '############
Else
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
End If
If _motor_stop = 1 Then
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_bl(4) = 62667 + _bl4offset
End If
_bl(4) = _bl(4) - _yaw_pid
End Sub
Sub Send_data()
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
Incr _timer2_counter
If _timer2_counter = 20 Then
_timer2_counter = 0
'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
'Print #2 , _yaw_kp ; ":" ; _roll_kp ; ":" ; _pitch_kp
'Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
'Print #2 , "ROLL: " ; _roll_kp_err ; ":" ; _roll_ki_sum ; ":" ; _roll_kd_err ;
'Print #2 , "PITCH: " ; _pitch_kp_err ; ":" ; _pitch_ki_sum ; ":" ; _pitch_kd_err ;
'Print #2 , "_______________"
End If
End Sub
Sub Init_system()
_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 2000
_stick_sensitivy(3) = 2000
_stick_sensitivy(4) = 2200 '1800
_empfmiddle(1) = 24500 '26500
_empfmiddle(2) = 23200 '23800
_empfmiddle(3) = 24950 '25300
_empfmiddle(4) = 22800 '22250
_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750
_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500
_yawnow = 0
_rollnow = 0
_pitchnow = 0
_failsave = 0
Reset _motor_stop
For I = 1 To 4
_empfdiv(i) = _empfmiddle(i) - _empfmin(i)
_empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i) '2265
_empfdiv(i) = _empfdiv(i) * 2
Next I
Call Wmp_init()
Wait 1
For I = 1 To 50 'the first measurements are trash!
Call Read_wmp_data()
Next I
Wait 1
Call Set_wmp_offset()
End Sub
Sub Failsave()
If Channel > 11 Then
Incr _failsave
Else
Decr _failsave
End If
If _failsave > 65000 Then _failsave = 55
If _failsave > 50 Then Set _motor_stop
If _failsave < 50 Then Reset _motor_stop
End Sub
Send_info:
Timer2 = 6
Incr _timer2_counter
If _timer2_counter = 500 Then
_timer2_counter = 0
'Print #2 , _rolloffset_int ; ":" ; _rollnow ; ":" ; _roll_pid
'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
End If
Return
End
Vielen Dank und Gruß
Chris
mein Tricopter funktioniert eigentlich ganz gut, jedoch wenn ich zu stark steuere, dann fängt er kurz an stark zu Taumeln! Wie kann ich dieses Taumeln unterdrücken? Mit dem D-Anteil? Allerdings weiß ich nicht, ob ich diesen positiv oder negativ einstellen soll? Habe schon beide Varianten gesehen! Kann mir jemand erklären, wo der Unterschied liegt? Habe leider nichts im Internet darüber gefunden..
Hier der Code:
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400
Open "COMC.2:19200,8,N,1" For Input As #1
Open "COMC.3:19200,8,N,1" For Output As #2
Declare Sub Failsave()
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 '0
Config Sda = Portc.4 '1
Config Twi = 100000
I2cinit
Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1
Config Int0 = Falling
On Int0 Measure
Enable Int0
Config Pind.2 = Input
Portd.2 = 0
Config Timer2 = Timer , Prescale = 64
Timer2 = 6
On Timer2 Send_info
Disable Timer2
Stop Timer2
'Start Timer2
'Enable Timer2
Dim _timer2_counter As Word
'####################################
'###########KONSTANTEN###############
'####################################
Const _maxchannel = 4
Const Start_byte = 127
Dim _yaw_kp As Integer
Dim _roll_kp As Integer
Dim _pitch_kp As Integer
Dim _yaw_ki As Integer
Dim _roll_ki As Integer
Dim _pitch_ki As Integer
Dim _yaw_kd As Integer
Dim _roll_kd As Integer
Dim _pitch_kd As Integer
_yaw_kp = 10 '10
_roll_kp = 4 '4
_pitch_kp = 8 '8
_yaw_ki = 5 '5
_roll_ki = 9 '9 'evtl. i groesser!!!
_pitch_ki = 13 '13
_yaw_kd = 0 '0
_roll_kd = -4 '-4
_pitch_kd = -4 '-4
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = 100
'###################################
'###################################
'###################################
'###################################
'#########FAILSAVE##################
'###################################
Dim _failsave As Word
Dim _motor_stop As Bit
'###################################
'###################################
'###################################
'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer
Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word
'###################################
'###################################
'###################################
'###################################
'###########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 _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer
Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer
Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer
Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer
Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer
Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer
Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer
'#################################
'#################################
'#################################
'#################################
'#########IIR-FILTER##############
'#################################
Dim _yaw_filter As Integer
Dim _yaw_filter_2 As Integer
Dim _roll_filter As Integer
Dim _roll_filter_2 As Integer
Dim _pitch_filter As Integer
Dim _pitch_filter_2 As Integer
Dim _sbl_filter(4) As Integer
Dim _sbl_filter_2(4) As Integer
'#################################
'#################################
'#################################
Wait 2
Call Init_system()
Enable Interrupts
Do
'Call Failsave
Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
'_motor_stop = 1
Call Mixer()
Call Send_data()
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()
For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I
'_sbl(2) = _sbl(2) * -1
'_sbl(3) = _sbl(3) * -1
'_sbl(4) = _sbl(4) * -1
_sbl(2) = _sbl(2) * 10 'PITCH
_sbl(2) = _sbl(2) / 110 '110
_sbl(3) = _sbl(3) * 10 'ROLL
_sbl(3) = _sbl(3) / 110 '130
_sbl(4) = _sbl(4) * 10 'YAW
_sbl(4) = _sbl(4) / 30 '30
'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)
'(
For I = 1 To 4
_sbl_filter(i) = _sbl_filter(i) * 6
_sbl_filter(i) = _sbl_filter(i) / 10
_sbl_filter_2(i) = _sbl(i) * 4
_sbl_filter_2(i) = _sbl_filter_2(i) / 10
_sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
_sbl(i) = _sbl_filter(i)
Next I
')
End Sub
Measure:
If Channel > 0 And Channel < 5 Then
Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return
Pausedetect:
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
'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)
'(
_yaw_filter = _yaw_filter * 4
_yaw_filter = _yaw_filter / 10
_yaw_filter_2 = Yaw * 6
_yaw_filter_2 = _yaw_filter_2 / 10
_yaw_filter = _yaw_filter + _yaw_filter_2
Yaw = _yaw_filter
_roll_filter = _roll_filter * 4
_roll_filter = _roll_filter / 10
_roll_filter_2 = Roll * 6
_roll_filter_2 = _roll_filter_2 / 10
_roll_filter = _roll_filter + _roll_filter_2
Roll = _roll_filter
_pitch_filter = _pitch_filter * 4
_pitch_filter = _pitch_filter / 10
_pitch_filter_2 = Pitch * 6
_pitch_filter_2 = _pitch_filter_2 / 10
_pitch_filter = _pitch_filter + _pitch_filter_2
Pitch = _pitch_filter
')
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
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()
'##############FEHLER_BERECHNEN##########
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow
_roll_err = _sbl(3) / 1
_roll_err = _roll_err - _rollnow
_pitch_err = _sbl(2) / 1
_pitch_err = _pitch_err - _pitchnow
'##############PROPORTIONAL##############
_yaw_kp_err = _yaw_err * _yaw_kp
_yaw_kp_err = _yaw_kp_err / 20 '20
_roll_kp_err = _roll_err * _roll_kp
_roll_kp_err = _roll_kp_err / 20
_pitch_kp_err = _pitch_err * _pitch_kp
_pitch_kp_err = _pitch_kp_err / 20
'##############INTEGRAL##################
_yaw_ki_err = _yaw_err * _yaw_ki
_yaw_ki_err = _yaw_ki_err / 2000 '2000
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_roll_ki_err = _roll_err * _roll_ki
_roll_ki_err = _roll_ki_err / 2000
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_pitch_ki_err = _pitch_err * _pitch_ki
_pitch_ki_err = _pitch_ki_err / 2000
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
'##############DIFFERENNTIAL#############
_yaw_kd_err = _yaw_err * _yaw_kd
_yaw_kd_err = _yaw_kd_err / 4000 '4000
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_err
_roll_kd_err = _roll_err * _roll_kd
_roll_kd_err = _roll_kd_err / 4000
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_err
_pitch_kd_err = _pitch_err * _pitch_kd
_pitch_kd_err = _pitch_kd_err / 4000
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_err
'##############AUFSUMMIEREN##############
_yaw_pid = _yaw_kp_err + _yaw_ki_sum
_yaw_pid = _yaw_pid + _yaw_kd_err
_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err
_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err
'###############WERTE_BEGRENZEN##########
If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub
Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset
If _sbl(1) > -920 Then
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_pitch_pid = _pitch_pid / 2 '############
_bl(2) = _bl(2) - _pitch_pid '############
_bl(3) = _bl(3) - _pitch_pid '############
Else
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
End If
If _motor_stop = 1 Then
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_bl(4) = 62667 + _bl4offset
End If
_bl(4) = _bl(4) - _yaw_pid
End Sub
Sub Send_data()
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
Incr _timer2_counter
If _timer2_counter = 20 Then
_timer2_counter = 0
'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
'Print #2 , _yaw_kp ; ":" ; _roll_kp ; ":" ; _pitch_kp
'Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
'Print #2 , "ROLL: " ; _roll_kp_err ; ":" ; _roll_ki_sum ; ":" ; _roll_kd_err ;
'Print #2 , "PITCH: " ; _pitch_kp_err ; ":" ; _pitch_ki_sum ; ":" ; _pitch_kd_err ;
'Print #2 , "_______________"
End If
End Sub
Sub Init_system()
_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 2000
_stick_sensitivy(3) = 2000
_stick_sensitivy(4) = 2200 '1800
_empfmiddle(1) = 24500 '26500
_empfmiddle(2) = 23200 '23800
_empfmiddle(3) = 24950 '25300
_empfmiddle(4) = 22800 '22250
_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750
_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500
_yawnow = 0
_rollnow = 0
_pitchnow = 0
_failsave = 0
Reset _motor_stop
For I = 1 To 4
_empfdiv(i) = _empfmiddle(i) - _empfmin(i)
_empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i) '2265
_empfdiv(i) = _empfdiv(i) * 2
Next I
Call Wmp_init()
Wait 1
For I = 1 To 50 'the first measurements are trash!
Call Read_wmp_data()
Next I
Wait 1
Call Set_wmp_offset()
End Sub
Sub Failsave()
If Channel > 11 Then
Incr _failsave
Else
Decr _failsave
End If
If _failsave > 65000 Then _failsave = 55
If _failsave > 50 Then Set _motor_stop
If _failsave < 50 Then Reset _motor_stop
End Sub
Send_info:
Timer2 = 6
Incr _timer2_counter
If _timer2_counter = 500 Then
_timer2_counter = 0
'Print #2 , _rolloffset_int ; ":" ; _rollnow ; ":" ; _roll_pid
'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
End If
Return
End
Vielen Dank und Gruß
Chris