Che Guevara
21.05.2011, 22:35
Hallo,
nach nur kurzer Zeit melde ich mich gleich mal wieder :)
Habs jetzt hinbekommen, meinen Tricopter komplett fertig zu machen bis auf das Chassis und die Regel-Parameter müssen noch genauer eingestellt werden, aber so im Groben und Ganzen läuft das Teil prima :D
Allerdings habe ich ein Problem. Momentan regele ich mit meinem PID-Regler nicht die Winkelgeschwindigkeit, sondern den Winkel; Sprich:
Wenn ich z.b. den Yaw-Knüppel in eine Richtung schiebe und dann wider zurück in Nullposition, dann bleibt der Yaw-Servo solange in dieser "verschobenen" Lage, bis ich den Knüppel wieder in die andere Richtung (um den gleichen Wert) schiebe... Ich möchte aber, dass der Servo sich je stärker dreht, je stärker man den Knüppel bewegt! Aber ich komme nicht drauf :(
Hier mal mein jetziger Code:
$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 140
$hwstack = 120
$swstack = 120
$baud = 38400
Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()
$lib "I2C_TWI.LBX" 'Hardware I2C
Config Scl = Portc.5 'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000 '400000
I2cinit
Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1
Config Int1 = Falling
On Int1 Measure
Enable Int1
Config Pind.3 = Input
Portd.3 = 0
Const Start_byte = 127
Const _maxchannel = 4
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600
Const _yaw_kp = 2
Const _roll_kp = 2
Const _pitch_kp = 2
Const _yaw_ki = 1
Const _roll_ki = 1
Const _pitch_ki = 1
Const _yaw_kd = 3
Const _roll_kd = 3
Const _pitch_kd = 3
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
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 Buffer(6) As Byte
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 _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow 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
_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 1800
_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 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
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()
Waitms 500
Call Set_offset()
Enable Interrupts
Do
For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I
_sbl(4) = _sbl(4) * -1
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow
_roll_err = _sbl(3) / 5
_roll_err = _roll_err - _rollnow
_pitch_err = _sbl(2) / 5
_pitch_err = _pitch_err - _pitchnow
_yaw_kp_err = _yaw_err / 10
_yaw_kp_err = _yaw_kp_err * _yaw_kp
_roll_kp_err = _roll_err / 10
_roll_kp_err = _roll_kp_err * _roll_kp
_pitch_kp_err = _pitch_err / 10
_pitch_kp_err = _pitch_kp_err * _pitch_kp
_yaw_ki_err = _yaw_err / 50
_yaw_ki_err = _yaw_ki_err * _yaw_ki
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_roll_ki_err = _roll_err / 50
_roll_ki_err = _roll_ki_err * _roll_ki
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_pitch_ki_err = _pitch_err / 50
_pitch_ki_err = _pitch_ki_err * _pitch_ki
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
_yaw_kd_err = _yaw_err / 50
_yaw_kd_err = _yaw_kd_err * _yaw_kd
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_old
_roll_kd_err = _roll_err / 50
_roll_kd_err = _roll_kd_err * _roll_kd
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_old
_pitch_kd_err = _pitch_err / 50
_pitch_kd_err = _pitch_kd_err * _pitch_kd
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_old
_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
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
_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
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_bl(4) = _bl(4) - _yaw_pid
If Channel <= 11 Then
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End If
Loop
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 ' sends memory address
I2cwbyte &HFE ' WM+ activation
I2cwbyte &H04 . ' Now Adress changes to &HA4
I2cstop
End Sub
Sub Send_zero()
I2cstart
I2cwbyte &HA4 ' sends memory address
I2cwbyte &H00 ' sends zero before receiving
I2cstop
Waitms 1
End Sub
Sub Read_data()
Gosub Send_zero ' sends zero before receiving
I2creceive &HA4 , Buffer(1) , 0 , 6 ' receive 6 bytes
Yaw1 = Buffer(1)
Roll1 = Buffer(2) ' Low Bytes
Pitch1 = Buffer(3)
Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5) ' High Bytes
Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub
Sub Set_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 50
Call Read_data
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 50
_rolloffset = _rolloffset / 50
_pitchoffset = _pitchoffset / 50
End Sub
End
Ich denke, das sollte eig kein Problem sein, evtl. steh ich grad auf der Leitung. Wäre nett, wenn mir jemand nen Denkanstoß geben könnte :)
Gruß
Chris
EDIT:
Sry, mir ist gerade aufgefallen, dass ich mich vertan habe! Momentan teste ich den Tricopter ohne Luftschrauben (da eine gerade "aushärtet", also der 2Komponenten-Kleber), deswegen sind die Motoren alle nicht wieder auf ihre ürsprüngliche Geschwindigkeit zurückgegangen, da sie ja auf eine Drehung warten... Wenn ich den Tricopter nun drehe, stimmt alles!
nach nur kurzer Zeit melde ich mich gleich mal wieder :)
Habs jetzt hinbekommen, meinen Tricopter komplett fertig zu machen bis auf das Chassis und die Regel-Parameter müssen noch genauer eingestellt werden, aber so im Groben und Ganzen läuft das Teil prima :D
Allerdings habe ich ein Problem. Momentan regele ich mit meinem PID-Regler nicht die Winkelgeschwindigkeit, sondern den Winkel; Sprich:
Wenn ich z.b. den Yaw-Knüppel in eine Richtung schiebe und dann wider zurück in Nullposition, dann bleibt der Yaw-Servo solange in dieser "verschobenen" Lage, bis ich den Knüppel wieder in die andere Richtung (um den gleichen Wert) schiebe... Ich möchte aber, dass der Servo sich je stärker dreht, je stärker man den Knüppel bewegt! Aber ich komme nicht drauf :(
Hier mal mein jetziger Code:
$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 140
$hwstack = 120
$swstack = 120
$baud = 38400
Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()
$lib "I2C_TWI.LBX" 'Hardware I2C
Config Scl = Portc.5 'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000 '400000
I2cinit
Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1
Config Int1 = Falling
On Int1 Measure
Enable Int1
Config Pind.3 = Input
Portd.3 = 0
Const Start_byte = 127
Const _maxchannel = 4
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600
Const _yaw_kp = 2
Const _roll_kp = 2
Const _pitch_kp = 2
Const _yaw_ki = 1
Const _roll_ki = 1
Const _pitch_ki = 1
Const _yaw_kd = 3
Const _roll_kd = 3
Const _pitch_kd = 3
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
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 Buffer(6) As Byte
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 _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow 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
_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 1800
_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 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
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()
Waitms 500
Call Set_offset()
Enable Interrupts
Do
For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I
_sbl(4) = _sbl(4) * -1
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow
_roll_err = _sbl(3) / 5
_roll_err = _roll_err - _rollnow
_pitch_err = _sbl(2) / 5
_pitch_err = _pitch_err - _pitchnow
_yaw_kp_err = _yaw_err / 10
_yaw_kp_err = _yaw_kp_err * _yaw_kp
_roll_kp_err = _roll_err / 10
_roll_kp_err = _roll_kp_err * _roll_kp
_pitch_kp_err = _pitch_err / 10
_pitch_kp_err = _pitch_kp_err * _pitch_kp
_yaw_ki_err = _yaw_err / 50
_yaw_ki_err = _yaw_ki_err * _yaw_ki
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_roll_ki_err = _roll_err / 50
_roll_ki_err = _roll_ki_err * _roll_ki
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_pitch_ki_err = _pitch_err / 50
_pitch_ki_err = _pitch_ki_err * _pitch_ki
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
_yaw_kd_err = _yaw_err / 50
_yaw_kd_err = _yaw_kd_err * _yaw_kd
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_old
_roll_kd_err = _roll_err / 50
_roll_kd_err = _roll_kd_err * _roll_kd
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_old
_pitch_kd_err = _pitch_err / 50
_pitch_kd_err = _pitch_kd_err * _pitch_kd
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_old
_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
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
_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
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_bl(4) = _bl(4) - _yaw_pid
If Channel <= 11 Then
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End If
Loop
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 ' sends memory address
I2cwbyte &HFE ' WM+ activation
I2cwbyte &H04 . ' Now Adress changes to &HA4
I2cstop
End Sub
Sub Send_zero()
I2cstart
I2cwbyte &HA4 ' sends memory address
I2cwbyte &H00 ' sends zero before receiving
I2cstop
Waitms 1
End Sub
Sub Read_data()
Gosub Send_zero ' sends zero before receiving
I2creceive &HA4 , Buffer(1) , 0 , 6 ' receive 6 bytes
Yaw1 = Buffer(1)
Roll1 = Buffer(2) ' Low Bytes
Pitch1 = Buffer(3)
Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5) ' High Bytes
Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub
Sub Set_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 50
Call Read_data
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 50
_rolloffset = _rolloffset / 50
_pitchoffset = _pitchoffset / 50
End Sub
End
Ich denke, das sollte eig kein Problem sein, evtl. steh ich grad auf der Leitung. Wäre nett, wenn mir jemand nen Denkanstoß geben könnte :)
Gruß
Chris
EDIT:
Sry, mir ist gerade aufgefallen, dass ich mich vertan habe! Momentan teste ich den Tricopter ohne Luftschrauben (da eine gerade "aushärtet", also der 2Komponenten-Kleber), deswegen sind die Motoren alle nicht wieder auf ihre ürsprüngliche Geschwindigkeit zurückgegangen, da sie ja auf eine Drehung warten... Wenn ich den Tricopter nun drehe, stimmt alles!