Multiplikation mit Konstante geht nicht

[Che Guevara]

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


_yawnow = _yawnow / 5
...
gehts, bei


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:


$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:


Dim _yaw_kp As Single
_yaw_kp = 0.5
...

Das geht auch nicht, hier gehts nicht mal mit _yaw_kp >= 0.6!
komisch, komisch...

[for_ro]

Es scheint so, als ob dann _yawnow Null wird, aber warum?
...
Dim _yawnow As Integer

Weil _yawnow vom Typ Ganzzahl mit Vorzeichen ist, während 0.2 eine Dezimalzahl ist.
Wenn es dir nicht zu zeitintensiv ist, dann bleibe doch bei der Berechnung /5.

[Che Guevara]

Hm, also eine etwas bescheidene Eigenschaft von BASCOM...
Dann werd ich mal schaun, was kürzer ist:
- Entweder erst _yawnow in eine Dezimalzahl konvertieren oder
- erst durch 10 teilen und dann mit _xxx_kp multiplizeren (also dann um den Faktor 10 größer)

Vielen Dank
Gruß
Chris

EDIT:
Habs gerade ausprobiert:


Const _yaw_kp = 5
Const _yaw_single_kp = 0.5
...
...
...

'330Cycles:
_yawnow = 800

_yawnow = _yawnow / 10
_yawnow = _yawnow * _yaw_kp
'########

'795Cycles
_yawnow = 800

_yawnow_single = _yawnow
_yawnow = _yawnow_single * _yaw_single_kp
'########

Also werd ich die Werte woll erst durch 10 teilen... Danke für die Hilfe

[Michael]

Hallo cheguevara,


Hm, also eine etwas bescheidene Eigenschaft von BASCOM...
wohl eher vom Programmierer.
Eine Ganzzahl bleibt auch in anderen Sprachen eine Ganzzahl.

Gruß, Michael

[Che Guevara]

Ja, das ist mir klar, aber warum kann BASCOM das dann nicht automatisch so compilieren, dass die Multiplikation funktioniert? Sonst macht es doch auch alles von alleine, z.b. Push & Pop, usw...
Aber darum gehts eigtl. gar nicht, den wenn mans weiß ist es kein Problem mehr!
Trotzdem muss ich dir Recht geben ;)

Gruß
Chris

[wkrug]

Wenn Du mit ganzzahlen arbeitest ist es eventuell sinnvoller, wenn Du die Multiplikation vor der Division ausführst.

Das braucht nicht mehr Rechenzeit, erhöht aber die Präzision, weil weniger Reste unter den Tisch fallen.

Allerdings musst Du dann aufpassen, das der maximale Wertebereich deiner Variable nicht überschritten wird!

[Che Guevara]

Ja, das dachte ich mir auch schon, trotzdem danke für den Tip :)

Gruß
Chris