Hallo,
nach meinem letzten Thread hat sich bei meinem Tricopter einiges getan! Er ist etwas leichter geworden (trotzdem noch viiiiiiel zu schwer) und ich habe einen Fehler gefunden:
I-wie werden die jetzigen Yaw-Werte nicht richtig berechnet, sodass sich im Stillstand schon 2 Motoren drehen, obwohl sie normal still stehen sollten...
Wenn ich jetzt aber die Twi-Geschwindigkeit von 400000 auf 100000 setze, gehts!
Jetzt tritt allerdings ein neues Problem auf:
Wenn ich schreibe
Code:
_yawnow = _yawnow / 5
...
gehts, bei
Code:
const _yaw_kp = 0.2
...
_yawnow = _yawnow * _yaw_kp
funktionierts nicht?! ...
Es scheint so, als ob dann _yawnow Null wird, aber warum?
Hier mal der ganze Code:
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
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()
'Servo-Funktion:
'min: 63800, mitte: 62667.5 , max: 61535 --> 2265 Schritte
Dim _error_yaw As Integer
Dim _error_roll As Integer
Dim _error_pitch As Integer
Dim _ist_yaw As Integer
Dim _ist_roll As Integer
Dim _ist_pitch As Integer
Dim _soll_yaw As Integer
Dim _soll_roll As Integer
Dim _soll_pitch As Integer
Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid 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
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600
Const _yaw_kp = 0.5
Const _roll_kp = 0.5
Const _pitch_kp = 0.5
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
_sbl(2) = _sbl(2) / 3
_sbl(3) = _sbl(3) / 3
_sbl(4) = _sbl(4) / 2
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1) + _sbl(3)
_bl(3) = _bl(1) - _sbl(3)
_bl(1) = _bl(1) + _sbl(2)
_bl(4) = 62667 + _sbl(4)
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset
_yawnow = _yawnow / 5
_rollnow = _rollnow / 5
_pitchnow = _pitchnow / 5
If _yawnow > 1000 Then _yawnow = 1000
If _yawnow < -1000 Then _yawnow = -1000
If _rollnow > 1000 Then _rollnow = 1000
If _rollnow < -1000 Then _rollnow = -1000
If _pitchnow > 1000 Then _pitchnow = 1000
If _pitchnow < -1000 Then _pitchnow = -1000
_bl(1) = _bl(1) - _pitchnow
_bl(2) = _bl(2) - _rollnow
_bl(3) = _bl(3) + _rollnow
_bl(4) = _bl(4) + _yawnow
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
Jeder kann sich wahrscheinlich denken, wo die zwei oberen Beispiele hingehören 
Vielen Dank schon mal
Gruß
Chris
EDIT:
Mir ist gerade noch was aufgefallen:
Bei _yaw_kp >= 0.6 gehts, d.h. ich habe bei einen kleinen Drehung des Kopters schon einen Endanschlag des Servos...
Bei _yaw_kp < 0.6 rührt sich überhaupt nichts?! ....
Bei den anderen ist es genauso..
Ich bin mir allerdings nicht ganz sicher, ob sich der Ausschlag des Servos / der BL-Motoren ändert, wenn die Werte verändert werden...
Kann es sein, dass da Bascom irgendwie rundet? Oder habe ich einfach nur einen Fehler im Programm?
EDIT2:
Gerade probiert:
Code:
Dim _yaw_kp As Single
_yaw_kp = 0.5
...
Das geht auch nicht, hier gehts nicht mal mit _yaw_kp >= 0.6!
komisch, komisch...
ZitierenZitat von Che Guevara
Berechtigungen
Lesezeichen