-
        

Ergebnis 1 bis 6 von 6

Thema: Shift bei Long

  1. #1
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544

    Shift bei Long

    Anzeige

    Hallo,

    um eine Prozedur etwas schneller zu machen, wollte ich versch. Divisionen durch Shiften ersetzen, jedoch habe ich ein Problem:
    So wollte ich es machen:
    Code:
    Shift _long_tmp , Right, 15, Signed
    Jedoch scheint das nicht zu funktionieren, warum auch immer?! Die Variable _long_tmp kann sowohl negativ als auch positiv sein. Ist das ein Bug in meiner Version (V1.11.9.5) oder habe ich einen Fehler?
    Zum Hintergrund:
    Im Code meines Tricopters habe ich einen PID-Regler, welcher mit Integern rechnet, weils schneller als Fließkomma ist. Ich multipliziere den Fehler erst mit der jeweiligen Variable und teile dann durch einen best. Wert... dieses Teilen möchte ich eben durch Shift ersetzen. Damit mir meine Integer Variable nicht überlauft, benutze ich dafür die Variable _long_tmp. Hier mal der komplette Code:
    Code:
    $regfile = "m328pdef.dat"
    $crystal = 16000000
    $framesize = 200
    $hwstack = 200
    $swstack = 200
    $baud = 38400
    
    
    Open "COMC.2:19200,8,N,1" For Input As #1
    Open "COMC.3:19200,8,N,1" For Output As #2
    
    
    Declare Sub Init_system()
    
    Declare Sub Filter_gyro_data()
    Declare Sub Filter_rx_data()
    Declare Sub Mixer()
    Declare Sub Send_data()
    
    Declare Sub Pid_regulator()
    
    Declare Sub Wmp_init()
    Declare Sub Send_zero()
    Declare Sub Read_wmp_data()
    Declare Sub Set_wmp_offset()
    
    
    
    $lib "I2C_TWI.LBX"
    Config Scl = Portc.5                                        '0
    Config Sda = Portc.4                                        '1
    Config Twi = 100000
    I2cinit
    
    
    Config Timer0 = Timer , Prescale = 256
    On Timer0 Pausedetect
    Enable Timer0
    
    Config Int0 = Falling
    On Int0 Measure
    Enable Int0
    
    
    Config Pind.2 = Input
    Portd.2 = 0
    
    
    '####################################
    '###########KONSTANTEN###############
    '####################################
    Const _maxchannel = 8
    Const Start_byte = 127
    
    
    Dim _yaw_kp As Integer
    Dim _roll_kp As Integer
    Dim _pitch_kp As Integer
    
    Dim _yaw_ki As Integer
    Dim _roll_ki As Integer
    Dim _pitch_ki As Integer
    
    Dim _yaw_kd As Integer
    Dim _roll_kd As Integer
    Dim _pitch_kd As Integer
    
    
    _yaw_kp = 100                                               '100
    _roll_kp = 40                                               '40
    _pitch_kp = 85                                              '85
    
    _yaw_ki = 55                                                '55
    _roll_ki = 90                                               '90
    _pitch_ki = 138                                             '138
    
    _yaw_kd = 0                                                 '0
    _roll_kd = 0                                                '120
    _pitch_kd = 0                                               '112
    
    
    Const _bl1offset = 0
    Const _bl2offset = 0
    Const _bl3offset = 0
    Const _bl4offset = 0
    '###################################
    '###################################
    '###################################
    
    
    
    Dim _long_tmp As Long
    
    Dim _timer2_counter As Byte
    
    
    
    '###################################
    '#########RC-EMPFÄNGER##############
    '###################################
    Dim Bufferbyte As Byte
    Dim Kanal(_maxchannel) As Word
    Dim Channel As Byte
    Dim _bl(4) As Word
    Dim I As Byte
    Dim _crc As Word
    Dim _sbl(_maxchannel) As Integer
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '###########I2C-Inputs##############
    '###################################
    Dim Wmplus_buffer(6) As Byte
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '#########GYRO######################
    '###################################
    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 _yawoffset_int As Integer
    Dim _rolloffset_int As Integer
    Dim _pitchoffset_int As Integer
    
    Dim _yawnow As Integer
    Dim _rollnow As Integer
    Dim _pitchnow As Integer
    '###################################
    '###################################
    '###################################
    
    
    '##################################
    '#########PID-REGLER###############
    '##################################
    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
    '#################################
    '#################################
    '#################################
    
    
    Wait 2
    
    Call Init_system()
    
    Enable Interrupts
    
    
    
    Do
    
    
     Call Filter_rx_data()
     Call Filter_gyro_data()
     Call Pid_regulator()
     Call Mixer()
     Call Send_data()
    
    
    Loop
    
    
    Sub Filter_gyro_data()
     Call Read_wmp_data()
    
     _yawnow = Yaw - _yawoffset_int
     _rollnow = Roll - _rolloffset_int
     _pitchnow = Pitch - _pitchoffset_int
    End Sub
    
    
    Sub Filter_rx_data()
    
     For I = 1 To _maxchannel
      _sbl(i) = Kanal(i) - 100
      If _sbl(i) < 2 And _sbl(i) > -2 Then _sbl(i) = 0
      _sbl(i) = _sbl(i) * 25
     Next I
    
     _sbl(2) = _sbl(2) * 10
     _sbl(2) = _sbl(2) / 110
    
     _sbl(3) = _sbl(3) * 10
     _sbl(3) = _sbl(3) / 110
    
     _sbl(4) = _sbl(4) * 10
     _sbl(4) = _sbl(4) / 30
    
    
     _sbl(7) = _sbl(7) / 10
     _sbl(8) = _sbl(8) / 10
    
     _roll_kd = 0 + _sbl(7)
     _pitch_kd = 0 + _sbl(8)
    
    
    End Sub
    
    
    Measure:
    If Channel > 0 And Channel < 9 Then
      Kanal(channel) = Timer0
    End If
    Timer0 = 6
    Incr Channel
    Return
    
    Pausedetect:
    Channel = 0
    Return
    
    
    Sub Wmp_init()
       I2cstart
       I2cwbyte &HA6
       I2cwbyte &HFE
       I2cwbyte &H04
       I2cstop
    End Sub
    
    Sub Send_zero()
       I2cstart
       I2cwbyte &HA4
       I2cwbyte &H00
       I2cstop
    End Sub
    
    Sub Read_wmp_data()
       Gosub Send_zero
       I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
       Yaw1 = Wmplus_buffer(1)
       Roll1 = Wmplus_buffer(2)
       Pitch1 = Wmplus_buffer(3)
       Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
       Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
       Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
       Shift Yaw , Right , 2
       Shift Roll , Right , 2
       Shift Pitch , Right , 2
    End Sub
    
    Sub Set_wmp_offset()
       _yawoffset = 0
       _rolloffset = 0
       _pitchoffset = 0
       For I = 1 To 100
          Waitms 5
          Call Read_wmp_data()
          _yawoffset = _yawoffset + Yaw
          _rolloffset = _rolloffset + Roll
          _pitchoffset = _pitchoffset + Pitch
       Next I
       _yawoffset = _yawoffset / 100
       _rolloffset = _rolloffset / 100
       _pitchoffset = _pitchoffset / 100
       _yawoffset_int = _yawoffset
       _rolloffset_int = _rolloffset
       _pitchoffset_int = _pitchoffset
    End Sub
    
    
    Sub Pid_regulator()
    
    '##############FEHLER_BERECHNEN##########
    
     _yaw_err = _sbl(4) - _yawnow
    
     _roll_err = _sbl(2) - _rollnow
    
     _pitch_err = _sbl(3) - _pitchnow
    
    '##############PROPORTIONAL##############
    
     _long_tmp = _yaw_err * _yaw_kp
     _long_tmp = _long_tmp / 200
     _yaw_kp_err = _long_tmp
    
     _long_tmp = _roll_err * _roll_kp
     _long_tmp = _long_tmp / 200
     _roll_kp_err = _long_tmp
    
     _long_tmp = _pitch_err * _pitch_kp
     _long_tmp = _long_tmp / 200
     _pitch_kp_err = _long_tmp
    
    '##############INTEGRAL##################
    
     _long_tmp = _yaw_err * _yaw_ki
     _long_tmp = _long_tmp / 20000
     _yaw_ki_err = _long_tmp
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _long_tmp = _roll_err * _roll_ki
     _long_tmp = _long_tmp / 20000
     _roll_ki_err = _long_tmp
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _long_tmp = _pitch_err * _pitch_ki
     _long_tmp = _long_tmp / 20000
     _pitch_ki_err = _long_tmp
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    '##############DIFFERENNTIAL#############
    
     _long_tmp = _yaw_err * _yaw_kd
     _long_tmp = _long_tmp / 40000
     _yaw_kd_err = _long_tmp
     _yaw_kd_err = _yaw_kd_old - _yaw_kd_err
     _yaw_kd_old = _yaw_kd_err
    
    
     _long_tmp = _roll_err * _roll_kd
     _long_tmp = _long_tmp / 40000
     _roll_kd_err = _long_tmp
     _roll_kd_err = _roll_kd_old - _roll_kd_err
     _roll_kd_old = _roll_kd_err
    
    
     _long_tmp = _pitch_err * _pitch_kd
     _long_tmp = _long_tmp / 40000
     _pitch_kd_err = _long_tmp
     _pitch_kd_err = _pitch_kd_err - _pitch_kd_old
     _pitch_kd_old = _pitch_kd_err
    
    '##############AUFSUMMIEREN##############
    
     _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
    
    '###############WERTE_BEGRENZEN##########
    
     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
    
    End Sub
    
    
    Sub Mixer()
     _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
    
     If _sbl(5) > -250 Then
      _bl(1) = _bl(1) + _pitch_pid
      _bl(2) = _bl(2) - _roll_pid
      _bl(3) = _bl(3) + _roll_pid
      _pitch_pid = _pitch_pid / 2
      _bl(2) = _bl(2) - _pitch_pid
      _bl(3) = _bl(3) - _pitch_pid
      _bl(4) = _bl(4) - _yaw_pid
     Else
      _bl(1) = 63800
      _bl(2) = 63800
      _bl(3) = 63800
      _yaw_kp_err = 0
      _yaw_ki_sum = 0
      _yaw_kd_err = 0
      _yaw_ki_err = 0
      _yaw_kd_old = 0
      _roll_kp_err = 0
      _roll_ki_sum = 0
      _roll_kd_err = 0
      _roll_ki_err = 0
      _roll_kd_old = 0
      _pitch_kp_err = 0
      _pitch_ki_sum = 0
      _pitch_kd_err = 0
      _pitch_ki_err = 0
      _pitch_kd_old = 0
      _yaw_pid = 0
      _roll_pid = 0
      _pitch_pid = 0
     End If
    
    End Sub
    
    
    Sub Send_data()
      _crc = Crc16(_bl(1) , 4)
      Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
      Incr _timer2_counter
      If _timer2_counter = 20 Then
       _timer2_counter = 0
       Print #2 , _roll_kd ; ":" ; _pitch_kd
       'Print #2 , Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5) ; ":" ; Kanal(6) ; ":" ; Kanal(7) ; ":" ; Kanal(8)
       'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4) ; ":" ; _sbl(5) ; ":" ; _sbl(6) ; ":" ; _sbl(7) ; ":" ; _sbl(8)
       'Print #2 , _yaw_kp ; ":" ; _roll_kp ; ":" ; _pitch_kp
       'Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
       'Print #2 , "ROLL: " ; _roll_kp_err ; ":" ; _roll_ki_sum ; ":" ; _roll_kd_err ;
       'Print #2 , "PITCH: " ; _pitch_kp_err ; ":" ; _pitch_ki_sum ; ":" ; _pitch_kd_err ;
       'Print #2 , "_______________"
      End If
    End Sub
    
    Sub Init_system()
    
    _yawnow = 0
    _rollnow = 0
    _pitchnow = 0
    
    Call Wmp_init()
    Wait 1
    For I = 1 To 50                                             'the first measurements are trash!
    Call Read_wmp_data()
    Next I
    Wait 1
    Call Set_wmp_offset()
    
    End Sub
    
    End
    Wenn ichs mit einer normalen Division mache, funktionierts, sprich der Kopter bewegt sich nicht, wenn er still auf dem Tisch steht! Wenn ich aber die Shift-Befehle einbaue, fängt nach ein paar Sekunden des stillen Liegens ein Motor an sich zu drehen! An was kann das liegen?

    Gruß
    Chris

  2. #2
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    07.03.2011
    Beiträge
    1.401
    Zitat Zitat von Che Guevara Beitrag anzeigen
    An was kann das liegen?
    IMHO gilt nur bei positiven Zahlen, daß ein Right Shift einer Division entspricht.

    MfG Klebwax
    Strom fließt auch durch krumme Drähte !

  3. #3
    Erfahrener Benutzer Roboter Genie Avatar von Searcher
    Registriert seit
    07.06.2009
    Ort
    NRW
    Beiträge
    1.410
    Blog-Einträge
    101
    Soweit ich das überblicke, kann man die Division einer ganzen Zahl wie eine LONG nur durch Shift right ersetzten, wenn der Divisor eine Zweierpotenz ist.

    "Shift _long_tmp , Right, 15, Signed" bedeutet eine Division durch 2^15 ( 32768 ) unter Beibehaltung des Vorzeichens. Damit kannst Du Divisionen durch 32768 ersetzten. Das funktioniert in Bascom Vers. 1.11.9.8 auch mit negativen Dividenden - zumindest im Simulator.

    Divisionen durch Zweierpotenzen finde ich aber in Deinem Code nicht. Wirst also andere Ergebnisse mit Shift als mit einer Division herausbekommen; also anderes Verhalten des Kopters.

    PS solche "Experimente" kann man gut im Simulator mit kurzen Testprogrammen machen.

    Gruß
    Searcher
    Hoffentlich liegt das Ziel auch am Weg
    ..................................................................Der Weg zu einigen meiner Konstruktionen

  4. #4
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544
    IMHO gilt nur bei positiven Zahlen, daß ein Right Shift einer Division entspricht.
    Ok, dann muss ich wohl erst abfragen, ob die Zahl negativ ist oder nicht und anschließend wenn nötig erst positiv machen zum shiften.... Aber im Simulator hats auch mit negativen Zahlen funktioniert?! Mal wieder so eine Unwahrheit, die im Simulator zutage tritt? Ohne Signed ist das falsche Ergebnis im Simulator rausgekommen, mit Signed wars richtig?!

    Divisionen durch Zweierpotenzen finde ich aber in Deinem Code nicht. Wirst also andere Ergebnisse mit Shift als mit einer Division herausbekommen; also anderes Verhalten des Kopters.
    Ich weiß, dass das shiften um 15 einer Division durch 32768 entspricht! Deswegen hatte ich beim ausprobieren auch die Regler-Parameter verändert, sodass eigentlich das richtige Ergebnis rauskommen hätte müssen, sprich:
    Ich habe die erste Division durch 200 durch ein Shift 8 ersetzt und die anderen beiden Divisionen durch ein Shift 15.
    Dann habe ich die jeweiligen Parameter neu berechnet und eingesetzt. Jedoch funktionierts nicht..

    Gruß
    Chris

  5. #5
    Erfahrener Benutzer Roboter Genie Avatar von Searcher
    Registriert seit
    07.06.2009
    Ort
    NRW
    Beiträge
    1.410
    Blog-Einträge
    101
    Hab trotzdem noch ein bißchen mit dem Simulator gespielt.

    Dort bringt ein Shift auf eine negative Zahl ein um 1 niedrigeres Ergebnis als die entsprechende Division, wenn die Division nicht ganzzahlig aufgeht.

    temp = -65536
    SHIFT temp , right , 15 , signed
    ergibt -2

    temp = -65536
    temp = temp / 32768
    ergibt auch -2

    aber

    temp = -65535
    SHIFT temp , right , 15 , signed
    ergibt -2

    temp = -65535
    temp = temp / 32768
    ergibt -1


    Also doch nicht so ohne weiteres ersetzbar


    Gruß
    Searcher
    Hoffentlich liegt das Ziel auch am Weg
    ..................................................................Der Weg zu einigen meiner Konstruktionen

  6. #6
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544
    Hm, also ob das Ergebnis um 1 kleiner ist oder nicht spielt hier keine so große Rolle! Im Simulator hat das ganze bei mir auch gefunzt, nur aufm AVR eben nicht?! Evtl. kann das ja mal jemand bei sich ausprobieren?

    Gruß
    Chris

Ähnliche Themen

  1. Shift-Daten an Fremdpin ausgeben
    Von hardware.bas im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 2
    Letzter Beitrag: 22.02.2011, 20:16
  2. Frage zu Shift Register
    Von Scarabol im Forum Elektronik
    Antworten: 1
    Letzter Beitrag: 23.10.2009, 07:31
  3. unsigned volatile long ??
    Von Ali_Baba im Forum C - Programmierung (GCC u.a.)
    Antworten: 9
    Letzter Beitrag: 04.03.2009, 21:31
  4. Variable mit Shift Out übertragen
    Von BlaueLed im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 3
    Letzter Beitrag: 16.11.2007, 14:52
  5. Byte zu Long konvertieren
    Von hacker im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 1
    Letzter Beitrag: 14.05.2006, 18:10

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •