Hallo,
nachdem ich gestern ein paar Erfolge hatte, liefs heute leider nicht so gut... Ich habe das Gehäuse zusammengebaut, die Motoren und die Steuerung usw... befestigt und wollts mal ausprobieren. Jedoch habe ich ein paar Probleme:
Bei meinem ersten Code (hier gehts jetzt nur noch um den ATMEGA
funktionierte alles so mittelmäßig:
Code:
$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 115200
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 = 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
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
Call Wmp_init()
Waitms 500
Call Set_offset()
Enable Interrupts
'Empfänger:
'29000 < kanal(x) < 64000
'mitte = (64000 + 29000) / 2 = 93000 / 2 = 46500
'Ausschlag: 17500
'Servo-Funktion:
'min: 63800, mitte: 62667.5 , max: 61535 --> 2265 Schritte
Do
Call Read_data()
For I = 1 To 4
_sbl(i) = Kanal(i) - 17500
_sbl(i) = _sbl(i) / 15.4525386
Next I
_sbl(2) = _sbl(2) - 600
_sbl(3) = _sbl(3) - 500
_bl(1) = 62667.5 + _sbl(1)
_bl(2) = _bl(1) - _sbl(3)
_bl(3) = _bl(1) + _sbl(3)
_bl(1) = _bl(1) - _sbl(2)
'_bl(4) = 62535 + _sbl(4)
'##############################
_yawnow = Yaw - _yawoffset
_yawnow = _yawnow / 2
_rollnow = Roll - _rolloffset
_rollnow = _rollnow / 2
_pitchnow = Pitch - _pitchoffset
_pitchnow = _pitchnow / 2
'##############################
_bl(1) = _bl(1) - _pitchnow
_bl(2) = _bl(2) - _rollnow
_bl(3) = _bl(3) + _rollnow
_bl(4) = 62535 + _yawnow
_bl(4) = _bl(4) + _sbl(4)
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
'Waitms 100
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
Wenn ich den Tricopter i-wie gedreht habe, haben die Motoren und der Servo darauf reagiert, jedoch fing dann das Ganze zu schwingen an. Dann dachte ich mir, bevor ich da jetzt optimiere, baue ich lieber gleich einen PID-Regler ein. Gesagt, getan....
Hier mal der Code:
Code:
$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 115200
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 = 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
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 Signal(3) As Integer
Dim Pid(3) As Integer
Dim Kp_err(3) As Integer
Dim Ki_err(3) As Integer
Dim Kd_err(3) As Integer
Dim Ki_sum(3) As Integer
Dim Kd_old(3) As Integer
Dim Kp(3) As Single
Dim Ki(3) As Single
Dim Kd(3) As Single
Kp(1) = 0.10
Kp(2) = 1
Kp(3) = 1
Ki(1) = 0
Ki(2) = 0
Ki(3) = 0
Kd(1) = 0
Kd(2) = 0
Kd(3) = 0
Call Wmp_init()
Waitms 500
Call Set_offset()
Enable Interrupts
'Empfänger:
'29000 < kanal(x) < 64000
'mitte = (64000 + 29000) / 2 = 93000 / 2 = 46500
'Ausschlag: 17500
'Servo-Funktion:
'min: 63800, mitte: 62667.5 , max: 61535 --> 2265 Schritte
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Do
For I = 1 To 4
_sbl(i) = Kanal(i) - 17500
_sbl(i) = _sbl(i) / 15.4525386
Next I
_sbl(2) = _sbl(2) - 600
_sbl(3) = _sbl(3) - 500
_bl(1) = 62667.5 + _sbl(1)
_bl(2) = _bl(1) - _sbl(3)
_bl(3) = _bl(1) + _sbl(3)
_bl(1) = _bl(1) - _sbl(2)
_bl(4) = 62535 + _sbl(4)
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset
Signal(1) = _yawnow
Signal(2) = _rollnow
Signal(3) = _pitchnow
For I = 1 To 3
'Proportional
Kp_err(i) = Signal(i) * Kp(i)
'Kp_err(i) = Kp_err(i) / 10
'Integral
Ki_err(i) = Signal(i) * Ki(i)
'Ki_err(i) = Ki_err(i) / 10
Ki_sum(i) = Ki_sum(i) + Ki_err(i)
'Differential
Kd_err(i) = Signal(i) * Kd(i)
'Kd_err(i) = Kd_err(i) / 10
Kd_err(i) = Kd_old(i) - Kd_err(i)
Kd_old(i) = Kd_err(i)
'Aufsummieren
Pid(i) = Kp_err(i) + Ki_sum(i)
Pid(i) = Pid(i) + Kd_err(i)
Next I
'Pid(1) --> Yaw-PID
'Pid(2) --> Roll-PID
'Pid(3) --> Pitch-PID
_bl(1) = _bl(1) - Pid(3)
_bl(2) = _bl(2) - Pid(2)
_bl(3) = _bl(3) + Pid(2)
_bl(4) = _bl(4) + Pid(1)
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
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
ABER:
Egal welche Werte ich für Kp einsetze (hab von 0.1 bis 1000 alles probiert), die Motoren / der Servo reagieren auf keinerlei Bewegung, selbst wenn ich das Teil innerhalb von ca. 0.5sek um 180° drehe. Aber warum? Der Offset vom WM+ liegt bei ca. 8000, das wird auch richtig berechnet.. Nur mit dem PID-Regler klappts nicht?! Wer kann miir weiterhelfen?
Dann habe ich noch eine Frage:
Im Programm stehen die Maximal und Minimal Werte des Empfängers und der Servos. Eigentlich sollte man die Knüppelposition doch so umrechnen können:
Code:
For I = 1 To 4
_sbl(i) = Kanal(i) - 46500 '-17500 < _sbl(i) < +17500
_sbl(i) = _sbl(i) / 15.4525386 '-1132 < _sbl(i) < +1132
Next I
_bl(1) = 62667 - _sbl(1)
Jedoch laufen dann die Motoren nicht so wie sie sollen. Normalerweise sollte doch Motor 1 (_bl(1)) nicht laufen, wenn der 1. Stick unten ist und die volle Drehzahl erreichen, wenn der Stick oben ist.. Das ist bei mir aber nicht so. Habe ich einen Denkfehler?
Vielen Dank
Gruß
Chris
Zitieren
Der PID-Regler macht aber trotzdem noch nichts... Außerdem habe ich gerade ein für mich unerklärliches Phänomen entdeckt:
Lesezeichen