-         

Ergebnis 1 bis 1 von 1

Thema: Tricopter stabilisieren

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

    Tricopter stabilisieren

    Anzeige

    Hallo,

    mein Tricopter funktioniert eigentlich ganz gut, jedoch wenn ich zu stark steuere, dann fängt er kurz an stark zu Taumeln! Wie kann ich dieses Taumeln unterdrücken? Mit dem D-Anteil? Allerdings weiß ich nicht, ob ich diesen positiv oder negativ einstellen soll? Habe schon beide Varianten gesehen! Kann mir jemand erklären, wo der Unterschied liegt? Habe leider nichts im Internet darüber gefunden..
    Hier der 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 Failsave()
    
    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 Timer1 = Timer , Prescale = 1
    On Timer1 Pausedetect
    Enable Timer1
    
    Config Int0 = Falling
    On Int0 Measure
    Enable Int0
    
    
    Config Pind.2 = Input
    Portd.2 = 0
    
    
    Config Timer2 = Timer , Prescale = 64
    Timer2 = 6
    On Timer2 Send_info
    Disable Timer2
    Stop Timer2
    'Start Timer2
    'Enable Timer2
    
    Dim _timer2_counter As Word
    
    
    '####################################
    '###########KONSTANTEN###############
    '####################################
    Const _maxchannel = 4
    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 = 10                                                '10
    _roll_kp = 4                                                '4
    _pitch_kp = 8                                               '8
    
    _yaw_ki = 5                                                 '5
    _roll_ki = 9                                                '9  'evtl. i groesser!!!
    _pitch_ki = 13                                              '13
    
    _yaw_kd = 0                                                 '0
    _roll_kd = -4                                               '-4
    _pitch_kd = -4                                              '-4
    
    
    Const _bl1offset = 0
    Const _bl2offset = 0
    Const _bl3offset = 0
    Const _bl4offset = 100
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '#########FAILSAVE##################
    '###################################
    Dim _failsave As Word
    Dim _motor_stop As Bit
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '#########RC-EMPFÄNGER##############
    '###################################
    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 _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
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '###########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
    '#################################
    '#################################
    '#################################
    
    
    '#################################
    '#########IIR-FILTER##############
    '#################################
    Dim _yaw_filter As Integer
    Dim _yaw_filter_2 As Integer
    Dim _roll_filter As Integer
    Dim _roll_filter_2 As Integer
    Dim _pitch_filter As Integer
    Dim _pitch_filter_2 As Integer
    
    Dim _sbl_filter(4) As Integer
    Dim _sbl_filter_2(4) As Integer
    '#################################
    '#################################
    '#################################
    
    Wait 2
    
    Call Init_system()
    
    Enable Interrupts
    
    
    
    Do
    
    
     'Call Failsave
     Call Filter_rx_data()
     Call Filter_gyro_data()
     Call Pid_regulator()
     '_motor_stop = 1
     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 4
      _sbl(i) = Kanal(i) - _empfmiddle(i)
      _sbl(i) = _sbl(i) / _empfdiv(i)
     Next I
    
     '_sbl(2) = _sbl(2) * -1
     '_sbl(3) = _sbl(3) * -1
     '_sbl(4) = _sbl(4) * -1
    
     _sbl(2) = _sbl(2) * 10                                     'PITCH
     _sbl(2) = _sbl(2) / 110                                    '110
    
     _sbl(3) = _sbl(3) * 10                                     'ROLL
     _sbl(3) = _sbl(3) / 110                                    '130
    
     _sbl(4) = _sbl(4) * 10                                     'YAW
     _sbl(4) = _sbl(4) / 30                                     '30
    
     'IIR-Filter
     'filter_wert = filter_wert * s + wert * (1.0-s)
    '(
     For I = 1 To 4
      _sbl_filter(i) = _sbl_filter(i) * 6
      _sbl_filter(i) = _sbl_filter(i) / 10
      _sbl_filter_2(i) = _sbl(i) * 4
      _sbl_filter_2(i) = _sbl_filter_2(i) / 10
      _sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
      _sbl(i) = _sbl_filter(i)
     Next I
    ')
    End Sub
    
    
    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
       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
    
       'IIR-Filter
       'filter_wert = filter_wert * s + wert * (1.0-s)
    '(
        _yaw_filter = _yaw_filter * 4
        _yaw_filter = _yaw_filter / 10
        _yaw_filter_2 = Yaw * 6
        _yaw_filter_2 = _yaw_filter_2 / 10
        _yaw_filter = _yaw_filter + _yaw_filter_2
        Yaw = _yaw_filter
    
        _roll_filter = _roll_filter * 4
        _roll_filter = _roll_filter / 10
        _roll_filter_2 = Roll * 6
        _roll_filter_2 = _roll_filter_2 / 10
        _roll_filter = _roll_filter + _roll_filter_2
        Roll = _roll_filter
    
        _pitch_filter = _pitch_filter * 4
        _pitch_filter = _pitch_filter / 10
        _pitch_filter_2 = Pitch * 6
        _pitch_filter_2 = _pitch_filter_2 / 10
        _pitch_filter = _pitch_filter + _pitch_filter_2
        Pitch = _pitch_filter
    ')
    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) / 1
     _yaw_err = _yaw_err - _yawnow
    
     _roll_err = _sbl(3) / 1
     _roll_err = _roll_err - _rollnow
    
     _pitch_err = _sbl(2) / 1
     _pitch_err = _pitch_err - _pitchnow
    
    '##############PROPORTIONAL##############
     _yaw_kp_err = _yaw_err * _yaw_kp
     _yaw_kp_err = _yaw_kp_err / 20                             '20
    
     _roll_kp_err = _roll_err * _roll_kp
     _roll_kp_err = _roll_kp_err / 20
    
     _pitch_kp_err = _pitch_err * _pitch_kp
     _pitch_kp_err = _pitch_kp_err / 20
    
    '##############INTEGRAL##################
     _yaw_ki_err = _yaw_err * _yaw_ki
     _yaw_ki_err = _yaw_ki_err / 2000                           '2000
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _roll_ki_err = _roll_err * _roll_ki
     _roll_ki_err = _roll_ki_err / 2000
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _pitch_ki_err = _pitch_err * _pitch_ki
     _pitch_ki_err = _pitch_ki_err / 2000
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    '##############DIFFERENNTIAL#############
     _yaw_kd_err = _yaw_err * _yaw_kd
     _yaw_kd_err = _yaw_kd_err / 4000                           '4000
     _yaw_kd_err = _yaw_kd_old - _yaw_kd_err
     _yaw_kd_old = _yaw_kd_err
    
     _roll_kd_err = _roll_err * _roll_kd
     _roll_kd_err = _roll_kd_err / 4000
     _roll_kd_err = _roll_kd_old - _roll_kd_err
     _roll_kd_old = _roll_kd_err
    
     _pitch_kd_err = _pitch_err * _pitch_kd
     _pitch_kd_err = _pitch_kd_err / 4000
     _pitch_kd_err = _pitch_kd_old - _pitch_kd_err
     _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(1) > -920 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                              '############
     Else
      _bl(1) = 63800
      _bl(2) = 63800
      _bl(3) = 63800
      _yaw_kp_err = 0
      _yaw_ki_sum = 0
      _yaw_kd_err = 0
      _roll_kp_err = 0
      _roll_ki_sum = 0
      _roll_kd_err = 0
      _pitch_kp_err = 0
      _pitch_ki_sum = 0
      _pitch_kd_err = 0
     End If
    
     If _motor_stop = 1 Then
      _bl(1) = 63800
      _bl(2) = 63800
      _bl(3) = 63800
      _bl(4) = 62667 + _bl4offset
     End If
    
     _bl(4) = _bl(4) - _yaw_pid
    
    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 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
       '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()
    _stick_sensitivy(1) = 2265
    _stick_sensitivy(2) = 2000
    _stick_sensitivy(3) = 2000
    _stick_sensitivy(4) = 2200                                  '1800
    
    _empfmiddle(1) = 24500                                      '26500
    _empfmiddle(2) = 23200                                      '23800
    _empfmiddle(3) = 24950                                      '25300
    _empfmiddle(4) = 22800                                      '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
    
    _yawnow = 0
    _rollnow = 0
    _pitchnow = 0
    
    
    _failsave = 0
    Reset _motor_stop
    
    
    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()
    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
    
    Sub Failsave()
    If Channel > 11 Then
    Incr _failsave
    Else
    Decr _failsave
    End If
    If _failsave > 65000 Then _failsave = 55
    If _failsave > 50 Then Set _motor_stop
    If _failsave < 50 Then Reset _motor_stop
    End Sub
    
    
    Send_info:
    Timer2 = 6
    Incr _timer2_counter
    If _timer2_counter = 500 Then
    _timer2_counter = 0
    'Print #2 , _rolloffset_int ; ":" ; _rollnow ; ":" ; _roll_pid
    'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4)
    Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
    End If
    Return
    
    End
    Vielen Dank und Gruß
    Chris
    Geändert von Che Guevara (27.06.2011 um 22:39 Uhr)

Ähnliche Themen

  1. neues Chassis für Tricopter
    Von Che Guevara im Forum Mechanik
    Antworten: 2
    Letzter Beitrag: 04.07.2011, 17:45
  2. Servozittern am Tricopter
    Von jevermeister im Forum Elektronik
    Antworten: 20
    Letzter Beitrag: 04.07.2011, 15:36
  3. TriGuide - Tricopter Steuerplatine PCB
    Von Willa im Forum Vorstellung+Bilder+Ideen zu geplanten eigenen Projekten/Bots
    Antworten: 53
    Letzter Beitrag: 22.10.2009, 00:51
  4. Tricopter mit Koax
    Von Willa im Forum Vorstellung+Bilder+Ideen zu geplanten eigenen Projekten/Bots
    Antworten: 47
    Letzter Beitrag: 09.12.2008, 22:07
  5. 12V Stabilisieren
    Von ShadowPhoenix im Forum Elektronik
    Antworten: 23
    Letzter Beitrag: 15.04.2004, 11:04

Berechtigungen

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