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...