-         

Ergebnis 1 bis 1 von 1

Thema: PID-Regler regelt Winkel anstatt Winkelgeschwindigkeit

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

    [] PID-Regler regelt Winkel anstatt Winkelgeschwindigkeit

    Anzeige

    Hallo,

    nach nur kurzer Zeit melde ich mich gleich mal wieder
    Habs jetzt hinbekommen, meinen Tricopter komplett fertig zu machen bis auf das Chassis und die Regel-Parameter müssen noch genauer eingestellt werden, aber so im Groben und Ganzen läuft das Teil prima
    Allerdings habe ich ein Problem. Momentan regele ich mit meinem PID-Regler nicht die Winkelgeschwindigkeit, sondern den Winkel; Sprich:
    Wenn ich z.b. den Yaw-Knüppel in eine Richtung schiebe und dann wider zurück in Nullposition, dann bleibt der Yaw-Servo solange in dieser "verschobenen" Lage, bis ich den Knüppel wieder in die andere Richtung (um den gleichen Wert) schiebe... Ich möchte aber, dass der Servo sich je stärker dreht, je stärker man den Knüppel bewegt! Aber ich komme nicht drauf
    Hier mal mein jetziger 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
    
    Const _bl1offset = 0
    Const _bl2offset = 0
    Const _bl3offset = 0
    Const _bl4offset = -600
    
    Const _yaw_kp = 2
    Const _roll_kp = 2
    Const _pitch_kp = 2
    
    Const _yaw_ki = 1
    Const _roll_ki = 1
    Const _pitch_ki = 1
    
    Const _yaw_kd = 3
    Const _roll_kd = 3
    Const _pitch_kd = 3
    
    
    
    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
    
    
    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 _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
    
    
    Call Wmp_init()
    Waitms 500
    Call Set_offset()
    
    
    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
    
    
     Call Read_data()
    
     _yawnow = Yaw - _yawoffset
     _rollnow = Roll - _rolloffset
     _pitchnow = Pitch - _pitchoffset
    
    
     _yaw_err = _sbl(4) / 1
     _yaw_err = _yaw_err - _yawnow
    
     _roll_err = _sbl(3) / 5
     _roll_err = _roll_err - _rollnow
    
     _pitch_err = _sbl(2) / 5
     _pitch_err = _pitch_err - _pitchnow
    
    
     _yaw_kp_err = _yaw_err / 10
     _yaw_kp_err = _yaw_kp_err * _yaw_kp
    
     _roll_kp_err = _roll_err / 10
     _roll_kp_err = _roll_kp_err * _roll_kp
    
     _pitch_kp_err = _pitch_err / 10
     _pitch_kp_err = _pitch_kp_err * _pitch_kp
    
    
     _yaw_ki_err = _yaw_err / 50
     _yaw_ki_err = _yaw_ki_err * _yaw_ki
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _roll_ki_err = _roll_err / 50
     _roll_ki_err = _roll_ki_err * _roll_ki
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _pitch_ki_err = _pitch_err / 50
     _pitch_ki_err = _pitch_ki_err * _pitch_ki
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    
     _yaw_kd_err = _yaw_err / 50
     _yaw_kd_err = _yaw_kd_err * _yaw_kd
     _yaw_kd_err = _yaw_kd_old - _yaw_kd_err
     _yaw_kd_old = _yaw_kd_old
    
     _roll_kd_err = _roll_err / 50
     _roll_kd_err = _roll_kd_err * _roll_kd
     _roll_kd_err = _roll_kd_old - _roll_kd_err
     _roll_kd_old = _roll_kd_old
    
     _pitch_kd_err = _pitch_err / 50
     _pitch_kd_err = _pitch_kd_err * _pitch_kd
     _pitch_kd_err = _pitch_kd_old - _pitch_kd_err
     _pitch_kd_old = _pitch_kd_old
    
    
     _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
    
    
     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
    
     _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
    
     _bl(1) = _bl(1) + _pitch_pid
     _bl(2) = _bl(2) - _roll_pid
     _bl(3) = _bl(3) + _roll_pid
     _bl(4) = _bl(4) - _yaw_pid
    
    
     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
    Ich denke, das sollte eig kein Problem sein, evtl. steh ich grad auf der Leitung. Wäre nett, wenn mir jemand nen Denkanstoß geben könnte

    Gruß
    Chris

    EDIT:
    Sry, mir ist gerade aufgefallen, dass ich mich vertan habe! Momentan teste ich den Tricopter ohne Luftschrauben (da eine gerade "aushärtet", also der 2Komponenten-Kleber), deswegen sind die Motoren alle nicht wieder auf ihre ürsprüngliche Geschwindigkeit zurückgegangen, da sie ja auf eine Drehung warten... Wenn ich den Tricopter nun drehe, stimmt alles!
    Geändert von Che Guevara (22.05.2011 um 10:48 Uhr)

Ähnliche Themen

  1. L5973: VOut = VIn; Regelt nicht?
    Von Jaecko im Forum Elektronik
    Antworten: 4
    Letzter Beitrag: 29.11.2011, 15:20
  2. Regler für Abstandhaltung mittels Winkel der Radstellung
    Von semicolon im Forum Software, Algorithmen und KI
    Antworten: 2
    Letzter Beitrag: 26.07.2007, 11:33
  3. Anstatt Relais...
    Von oratus sum im Forum Elektronik
    Antworten: 8
    Letzter Beitrag: 04.03.2007, 16:23
  4. Antworten: 11
    Letzter Beitrag: 14.10.2004, 12:34
  5. Antworten: 3
    Letzter Beitrag: 24.11.2003, 18:26

Berechtigungen

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