-         

Seite 1 von 2 12 LetzteLetzte
Ergebnis 1 bis 10 von 11

Thema: Summensignal auslesen funzt nur mit Timer0, nicht mit Timer2!?

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

    Timer0 blockiert Timer2

    Anzeige

    SMARTPHONES & TABLETS-bis zu 77% RABATT-Kostenlose Lieferung-Aktuell | Cool | Unentbehrlich
    Hallo,

    da es mich schon länger gestört hat, auf meinem Quadrocopter 2 Platinen zu benötigen (WMP und Hauptplatine), habe ich mir jetzt eine neue Platine geätzt, auf welcher alle wichtigen Teile vorhanden sind (WMP, ATMEGA328P, RC-Empfänger). Natürlich habe ich vorher die Änderungen nicht ausprobiert, da ich jetzt nur noch einen µC habe und dieser die PWM selbst ausgibt (vorher hat der Master einem ATTINY2313 die 4 Pwm Werte über UART übergeben). Aufgrund dieser Änderung wollte ich jetzt Timer2 zum Auslesen des Summensignals benutzen, jedoch funktioniert das nicht... Habs gerade ausprobiert, wenn ich NUR Timer2 und Timer0 vertausche (einer ist für PWM, der andere fürs Summensignal), gehts.
    Hier der Code, der funktioniert:
    Code:
    $regfile = "m328pdef.dat"
    $crystal = 16000000
    $framesize = 250
    $hwstack = 250
    $swstack = 250
    $baud = 19200
    
    
    Config Serialin = Buffered , Size = 50 , Bytematch = 13
    
    
    Declare Sub Serial0charmatch()
    
    Declare Sub Init_system()
    
    Declare Sub Filter_gyro_data()
    Declare Sub Filter_rx_data()
    Declare Sub Mixer()
    
    Declare Sub Pid_regulator()
    
    Declare Sub Wmp_init()
    Declare Sub Send_zero()
    Declare Sub Read_wmp_data()
    Declare Sub Set_wmp_offset()
    
    Declare Sub Failsave()
    
    Declare Sub Set_pwm()
    
    
    
    Config Portc.2 = Output
    Config Portc.3 = Output
    Portc.2 = 0
    Portc.3 = 0
    
    
    Config Portb.1 = Output
    Config Portb.2 = Output
    Config Portd.5 = Output
    Config Portd.6 = Output
    
    
    Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    Pwm1a = 113
    Pwm1b = 113
    
    Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    Pwm2a = 113
    Pwm2b = 113
    
    
    Config Pind.2 = Input
    Portd.2 = 0
    
    Config Int0 = Falling
    On Int0 Measure
    Enable Int0
    
    Config Timer0 = Timer , Prescale = 256
    On Timer0 Pausedetect
    Enable Timer0
    
    
    $lib "I2C_TWI.LBX"
    Config Scl = Portc.5
    Config Sda = Portc.4
    Config Twi = 100000
    I2cinit
    
    
    Const Minthrottle = 20
    Const Maxchannel = 8
    Const _throttlechannel = 1
    Const _rollchannel = 2
    Const _pitchchannel = 3
    Const _yawchannel = 4
    Const _statechannel = 5
    Const _datachannel = 6
    
    Dim I As Byte
    Dim J As Byte
    Dim Newvalsreceived As Bit
    Dim _blink As Byte
    
    Dim _yaw_kp As Single
    Dim _roll_kp As Single
    Dim _pitch_kp As Single
    
    Dim _yaw_ki As Single
    Dim _roll_ki As Single
    Dim _pitch_ki As Single
    
    Dim _yaw_kd As Single
    Dim _roll_kd As Single
    Dim _pitch_kd As Single
    
    _yaw_kp = 0.18                                              '0.180
    _roll_kp = 0.18
    _pitch_kp = 0.18
    
    _yaw_ki = 0
    _roll_ki = 0
    _pitch_ki = 0
    
    _yaw_kd = 0
    _roll_kd = 0
    _pitch_kd = 0
    
    
    '###################################
    '#########RC-EMPFÄNGER##############
    '###################################
    Dim Bufferbyte As Byte
    Dim Kanal(maxchannel) As Word
    Dim Fkanal(maxchannel) As Word
    Dim Channel As Byte
    Dim _bl(4) As Word
    Dim _sbl(maxchannel) As Integer
    Dim State As Byte
    Dim Oldstate As Byte
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '###########I2C-Inputs##############
    '###################################
    Dim Wmplus_buffer(6) As Byte
    Dim Ar(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 Single
    Dim _roll_kp_err As Single
    Dim _pitch_kp_err As Single
    
    Dim _yaw_ki_err As Single
    Dim _roll_ki_err As Single
    Dim _pitch_ki_err As Single
    
    Dim _yaw_ki_sum As Single
    Dim _roll_ki_sum As Single
    Dim _pitch_ki_sum As Single
    
    Dim _yaw_kd_err As Single
    Dim _roll_kd_err As Single
    Dim _pitch_kd_err As Single
    
    Dim _yaw_kd_old As Single
    Dim _roll_kd_old As Single
    Dim _pitch_kd_old As Single
    
    Dim _yaw_pid_int As Integer
    Dim _roll_pid_int As Integer
    Dim _pitch_pid_int As Integer
    
    Dim _yaw_pid As Single
    Dim _roll_pid As Single
    Dim _pitch_pid As Single
    
    Dim _yaw_err_int As Integer
    Dim _roll_err_int As Integer
    Dim _pitch_err_int As Integer
    
    Dim _yaw_err As Single
    Dim _roll_err As Single
    Dim _pitch_err As Single
    '#################################
    '#################################
    '#################################
    
    Dim Yawstickvel As Integer
    Dim Rollstickvel As Integer
    Dim Pitchstickvel As Integer
    
    Dim Yawstickold As Integer
    Dim Rollstickold As Integer
    Dim Pitchstickold As Integer
    
    Dim _x1 As Single
    Dim _x2 As Single
    
    Dim Rc_on As Word
    
    Dim Failure As Byte
    
    
    
    Call Init_system()
    Waitms 500
    Enable Interrupts
    Waitms 500
    
    
    
    Rc_test:
    If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
       If Rc_on < 65000 Then Incr Rc_on
    Else
       If Rc_on > 0 Then Decr Rc_on
    End If
    If Rc_on < 500 Then
       Goto Rc_test
    End If
    
    
    
    
    '_bl(1) = rechts hinten  (US)
    '_bl(2) = links hinten   (GUS)
    '_bl(3) = rechts vorne   (GUS)
    '_bl(4) = links vorne    (US)
    
    'PWM = 113 --> 0.900us
    'PWM = 250 --> 2.000ms
    
    
    
    
    Do
    
    
       Call Filter_rx_data()
       Call Filter_gyro_data()
       Call Pid_regulator()
       Call Mixer()
       Call Failsave()
       Call Set_pwm()
    
    
    Loop
    
    
    Sub Serial0charmatch()
       Clear Serialin
    End Sub
    
    
    Sub Set_pwm()
       If _bl(1) > 250 Then _bl(1) = 250
       If _bl(1) < 113 Then _bl(1) = 113
       If _bl(2) > 250 Then _bl(2) = 250
       If _bl(2) < 113 Then _bl(2) = 113
       If _bl(3) > 250 Then _bl(3) = 250
       If _bl(3) < 113 Then _bl(3) = 113
       If _bl(4) > 250 Then _bl(4) = 250
       If _bl(4) < 113 Then _bl(4) = 113
       Pwm2a = _bl(4)                                           'links vorne (US)
       Pwm2b = _bl(2)                                           'links hinten (GUS)
       Pwm1a = _bl(3)                                           'rechts vorne (GUS)
       Pwm1b = _bl(1)                                           'rechts hinten (US)
       If Kanal(_datachannel) > 100 Then
         Print Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5)
       End If
    End Sub
    
    
    Sub Failsave()
      If Channel >= 11 Then
        If Failure < 255 Then
          Incr Failure
        End If
      End If
      If Channel < 11 Then
        If Failure > 0 Then
          Decr Failure
        End If
      End If
    End Sub
    
    
    Measure:
     If Channel > 0 And Channel < 9 Then
       Kanal(channel) = Timer0
     End If
     Timer0 = 6
     Incr Channel
    Return
    
    
    Pausedetect:
      If Channel <> 0 Then
        Newvalsreceived = 1
      End If
      Channel = 0
    Return
    
    
    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()
    
     If Newvalsreceived = 1 Then
       Newvalsreceived = 0
    
       For I = 1 To Maxchannel
         Fkanal(i) = Fkanal(i) * 7                              '3
         Fkanal(i) = Fkanal(i) + Kanal(i)
         Shift Fkanal(i) , Right , 3                            '2
         Kanal(i) = Fkanal(i)
       Next I
    
       If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
         _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
         _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
         If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
         If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
       End If
    
       For I = 2 To Maxchannel
         If Kanal(i) >= 60 And Kanal(i) < 141 Then
           _sbl(i) = Kanal(i) - 100
           If _sbl(i) => -1 And _sbl(i) < 2 Then _sbl(i) = 0
           _sbl(i) = _sbl(i) * 6
         End If
       Next I
    
       If Kanal(5) < 80 Then State = 0
       If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
       If Kanal(5) >= 115 Then State = 2
    
       If State = 0 Or Oldstate <> State Then
          Rollstickold = 0
          Pitchstickold = 0
       End If
    
       Yawstickvel = _sbl(_yawchannel) + Yawstickold
       Yawstickold = _sbl(_yawchannel)
       Rollstickvel = _sbl(_rollchannel) + Rollstickold
       Rollstickold = _sbl(_rollchannel)
       Pitchstickvel = _sbl(_pitchchannel) + Pitchstickold
       Pitchstickold = _sbl(_pitchchannel)
    
       Yawstickvel = Yawstickvel / 4
       Yawstickvel = Yawstickvel * 3
    
       If State < 2 Then
         Rollstickvel = Rollstickvel / 10
         Pitchstickvel = Pitchstickvel / 10
       Else
         Rollstickvel = Rollstickvel / 4
         Pitchstickvel = Pitchstickvel / 4
       End If
    
    '(
    '#########P-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.085
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.085
    
       _roll_kp = 2.80 + _x1
       _pitch_kp = _roll_kp
    
    '#########I-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.0005
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.0005
    
       _roll_ki = 0.0050 + _x1
       _pitch_ki = _roll_ki
    
    '#########D-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.00002
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.00002
    
       _roll_kd = 0.0008 + _x1
       _pitch_kd = _roll_kd
    ')
    
    
       If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
         Call Set_wmp_offset()
       End If
    
     Oldstate = State
    
     End If
    
    End Sub
    
    
    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 , 4
       Shift Roll , Right , 4
       Shift Pitch , Right , 4
    End Sub
    
    
    Sub Set_wmp_offset()
       _yawoffset = 0
       _rolloffset = 0
       _pitchoffset = 0
       For I = 1 To 100
          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_int = Yawstickvel - _yawnow
     _yaw_err = _yaw_err_int
    
     _roll_err_int = Rollstickvel - _rollnow
     _roll_err = _roll_err_int
    
     _pitch_err_int = Pitchstickvel - _pitchnow
     _pitch_err = _pitch_err_int
    
    '##############PROPORTIONAL##############
    
     _yaw_kp_err = _yaw_err * _yaw_kp
    
     _roll_kp_err = _roll_err * _roll_kp
    
     _pitch_kp_err = _pitch_err * _pitch_kp
    
    '##############INTEGRAL##################
    
     _yaw_ki_err = _yaw_err * _yaw_ki
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _roll_ki_err = _roll_err * _roll_ki
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _pitch_ki_err = _pitch_err * _pitch_ki
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    '##############DIFFERENTIAL#############
    
     _yaw_kd_err = _yaw_err * _yaw_kd
     _yaw_kd_err = _yaw_kd_err - _yaw_kd_old
     _yaw_kd_old = _yaw_kd_err
    
     _roll_kd_err = _roll_err * _roll_kd
     _roll_kd_err = _roll_kd_err - _roll_kd_old
     _roll_kd_old = _roll_kd_err
    
     _pitch_kd_err = _pitch_err * _pitch_kd
     _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
     _yaw_pid_int = _yaw_pid
    
     _roll_pid = _roll_kp_err + _roll_ki_sum
     _roll_pid = _roll_pid + _roll_kd_err
     _roll_pid_int = _roll_pid
    
     _pitch_pid = _pitch_kp_err + _pitch_ki_sum
     _pitch_pid = _pitch_pid + _pitch_kd_err
     _pitch_pid_int = _pitch_pid
    
    '###############WERTE_BEGRENZEN##########
    
     If _yaw_pid_int < -68 Then _yaw_pid_int = -68
     If _yaw_pid_int > 68 Then _yaw_pid_int = 68
     If _roll_pid_int < -68 Then _roll_pid_int = -68
     If _roll_pid_int > 68 Then _roll_pid_int = 68
     If _pitch_pid_int < -68 Then _pitch_pid_int = -68
     If _pitch_pid_int > 68 Then _pitch_pid_int = 68
    
    End Sub
    
    
    Sub Mixer()
    
      '_bl(1) = rechts hinten  (US)
      '_bl(2) = links hinten   (GUS)
      '_bl(3) = rechts vorne   (GUS)
      '_bl(4) = links vorne    (US)
    
     _bl(1) = 182 + _sbl(_throttlechannel)
     _bl(2) = _bl(1)
     _bl(3) = _bl(1)
     _bl(4) = _bl(1)
    
     If State <> 0 And Failure < 20 Then
    
       Portc.2 = 0
       Portc.3 = 1
    
       _bl(1) = _bl(1) + Minthrottle
       _bl(2) = _bl(2) + Minthrottle
       _bl(3) = _bl(3) + Minthrottle
       _bl(4) = _bl(4) + Minthrottle
    
       _bl(1) = _bl(1) - _pitch_pid_int
       _bl(2) = _bl(2) - _pitch_pid_int
       _bl(3) = _bl(3) + _pitch_pid_int
       _bl(4) = _bl(4) + _pitch_pid_int
    
       _bl(1) = _bl(1) + _roll_pid_int
       _bl(2) = _bl(2) - _roll_pid_int
       _bl(3) = _bl(3) + _roll_pid_int
       _bl(4) = _bl(4) - _roll_pid_int
    
       _bl(1) = _bl(1) - _yaw_pid_int
       _bl(2) = _bl(2) + _yaw_pid_int
       _bl(3) = _bl(3) + _yaw_pid_int
       _bl(4) = _bl(4) - _yaw_pid_int
    
     Else
    
       Portc.2 = 0
    
       _bl(1) = 113
       _bl(2) = 113
       _bl(3) = 113
       _bl(4) = 113
    
       Incr _blink
       If _blink = 100 Then
         Portc.3 = 1
         _blink = 101
       End If
       If _blink = 200 Then
         Portc.3 = 0
         _blink = 0
       End If
    
       _yaw_kp_err = 0
       _yaw_ki_sum = 0
       _yaw_kd_err = 0
       _yaw_ki_err = 0
       _roll_kp_err = 0
       _roll_ki_sum = 0
       _roll_kd_err = 0
       _roll_ki_err = 0
       _pitch_kp_err = 0
       _pitch_ki_sum = 0
       _pitch_kd_err = 0
       _pitch_ki_err = 0
       _yaw_pid_int = 0
       _roll_pid_int = 0
       _pitch_pid_int = 0
       _yaw_pid = 0
       _roll_pid = 0
       _pitch_pid = 0
       Yawstickvel = 0
       Rollstickvel = 0
       Pitchstickvel = 0
       Yawstickold = 0
       Rollstickold = 0
       Pitchstickold = 0
    
     End If
    
    End Sub
    
    
    Sub Init_system()
    
    _bl(1) = 113
    _bl(2) = 113
    _bl(3) = 113
    _bl(4) = 113
    
    _sbl(1) = -68
    _sbl(2) = 0
    _sbl(3) = 0
    _sbl(4) = 0
    _sbl(5) = -600
    _sbl(6) = -600
    _sbl(7) = 0
    _sbl(8) = 0
    
    Fkanal(1) = 63
    Fkanal(2) = 100
    Fkanal(3) = 100
    Fkanal(4) = 100
    Fkanal(5) = 63
    Fkanal(6) = 63
    Fkanal(7) = 100
    Fkanal(8) = 100
    
    Reset Newvalsreceived
    
    State = 0
    Oldstate = 0
    
    _yawnow = 0
    _rollnow = 0
    _pitchnow = 0
    
    Call Wmp_init()
    Waitms 500
    For I = 1 To 10
    Call Read_wmp_data()
    Next I
    Waitms 500
    Call Set_wmp_offset()
    
    Rc_on = 0
    
    End Sub
    
    End
    Hier der Code, der NICHT funktioniert:
    Code:
    $regfile = "m328pdef.dat"
    $crystal = 16000000
    $framesize = 250
    $hwstack = 250
    $swstack = 250
    $baud = 19200
    
    
    Config Serialin = Buffered , Size = 50 , Bytematch = 13
    
    
    Declare Sub Serial0charmatch()
    
    Declare Sub Init_system()
    
    Declare Sub Filter_gyro_data()
    Declare Sub Filter_rx_data()
    Declare Sub Mixer()
    
    Declare Sub Pid_regulator()
    
    Declare Sub Wmp_init()
    Declare Sub Send_zero()
    Declare Sub Read_wmp_data()
    Declare Sub Set_wmp_offset()
    
    Declare Sub Failsave()
    
    Declare Sub Set_pwm()
    
    
    
    Config Portc.2 = Output
    Config Portc.3 = Output
    Portc.2 = 0
    Portc.3 = 0
    
    
    Config Portb.1 = Output
    Config Portb.2 = Output
    Config Portd.5 = Output
    Config Portd.6 = Output
    
    
    Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    Pwm1a = 113
    Pwm1b = 113
    
    Config Timer0 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    Pwm0a = 113
    Pwm0b = 113
    
    
    Config Pind.2 = Input
    Portd.2 = 0
    
    Config Int0 = Falling
    On Int0 Measure
    Enable Int0
    
    Config Timer2 = Timer , Prescale = 256
    On Timer2 Pausedetect
    Enable Timer2
    
    
    $lib "I2C_TWI.LBX"
    Config Scl = Portc.5
    Config Sda = Portc.4
    Config Twi = 100000
    I2cinit
    
    
    Const Minthrottle = 20
    Const Maxchannel = 8
    Const _throttlechannel = 1
    Const _rollchannel = 2
    Const _pitchchannel = 3
    Const _yawchannel = 4
    Const _statechannel = 5
    Const _datachannel = 6
    
    Dim I As Byte
    Dim J As Byte
    Dim Newvalsreceived As Bit
    Dim _blink As Byte
    
    Dim _yaw_kp As Single
    Dim _roll_kp As Single
    Dim _pitch_kp As Single
    
    Dim _yaw_ki As Single
    Dim _roll_ki As Single
    Dim _pitch_ki As Single
    
    Dim _yaw_kd As Single
    Dim _roll_kd As Single
    Dim _pitch_kd As Single
    
    _yaw_kp = 0.18                                              '0.180
    _roll_kp = 0.18
    _pitch_kp = 0.18
    
    _yaw_ki = 0
    _roll_ki = 0
    _pitch_ki = 0
    
    _yaw_kd = 0
    _roll_kd = 0
    _pitch_kd = 0
    
    
    '###################################
    '#########RC-EMPFÄNGER##############
    '###################################
    Dim Bufferbyte As Byte
    Dim Kanal(maxchannel) As Word
    Dim Fkanal(maxchannel) As Word
    Dim Channel As Byte
    Dim _bl(4) As Word
    Dim _sbl(maxchannel) As Integer
    Dim State As Byte
    Dim Oldstate As Byte
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '###########I2C-Inputs##############
    '###################################
    Dim Wmplus_buffer(6) As Byte
    Dim Ar(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 Single
    Dim _roll_kp_err As Single
    Dim _pitch_kp_err As Single
    
    Dim _yaw_ki_err As Single
    Dim _roll_ki_err As Single
    Dim _pitch_ki_err As Single
    
    Dim _yaw_ki_sum As Single
    Dim _roll_ki_sum As Single
    Dim _pitch_ki_sum As Single
    
    Dim _yaw_kd_err As Single
    Dim _roll_kd_err As Single
    Dim _pitch_kd_err As Single
    
    Dim _yaw_kd_old As Single
    Dim _roll_kd_old As Single
    Dim _pitch_kd_old As Single
    
    Dim _yaw_pid_int As Integer
    Dim _roll_pid_int As Integer
    Dim _pitch_pid_int As Integer
    
    Dim _yaw_pid As Single
    Dim _roll_pid As Single
    Dim _pitch_pid As Single
    
    Dim _yaw_err_int As Integer
    Dim _roll_err_int As Integer
    Dim _pitch_err_int As Integer
    
    Dim _yaw_err As Single
    Dim _roll_err As Single
    Dim _pitch_err As Single
    '#################################
    '#################################
    '#################################
    
    Dim Yawstickvel As Integer
    Dim Rollstickvel As Integer
    Dim Pitchstickvel As Integer
    
    Dim Yawstickold As Integer
    Dim Rollstickold As Integer
    Dim Pitchstickold As Integer
    
    Dim _x1 As Single
    Dim _x2 As Single
    
    Dim Rc_on As Word
    
    Dim Failure As Byte
    
    
    
    Call Init_system()
    Waitms 500
    Enable Interrupts
    Waitms 500
    
    
    
    Rc_test:
    If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
       If Rc_on < 65000 Then Incr Rc_on
    Else
       If Rc_on > 0 Then Decr Rc_on
    End If
    If Rc_on < 500 Then
       Goto Rc_test
    End If
    
    
    
    
    '_bl(1) = rechts hinten  (US)
    '_bl(2) = links hinten   (GUS)
    '_bl(3) = rechts vorne   (GUS)
    '_bl(4) = links vorne    (US)
    
    'PWM = 113 --> 0.900us
    'PWM = 250 --> 2.000ms
    
    
    
    
    Do
    
    
       Call Filter_rx_data()
       Call Filter_gyro_data()
       Call Pid_regulator()
       Call Mixer()
       Call Failsave()
       Call Set_pwm()
    
    
    Loop
    
    
    Sub Serial0charmatch()
       Clear Serialin
    End Sub
    
    
    Sub Set_pwm()
       If _bl(1) > 250 Then _bl(1) = 250
       If _bl(1) < 113 Then _bl(1) = 113
       If _bl(2) > 250 Then _bl(2) = 250
       If _bl(2) < 113 Then _bl(2) = 113
       If _bl(3) > 250 Then _bl(3) = 250
       If _bl(3) < 113 Then _bl(3) = 113
       If _bl(4) > 250 Then _bl(4) = 250
       If _bl(4) < 113 Then _bl(4) = 113
       Pwm0a = _bl(4)                                           'links vorne (US)
       Pwm0b = _bl(2)                                           'links hinten (GUS)
       Pwm1a = _bl(3)                                           'rechts vorne (GUS)
       Pwm1b = _bl(1)                                           'rechts hinten (US)
       If Kanal(_datachannel) > 100 Then
         Print Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5)
       End If
    End Sub
    
    
    Sub Failsave()
      If Channel >= 11 Then
        If Failure < 255 Then
          Incr Failure
        End If
      End If
      If Channel < 11 Then
        If Failure > 0 Then
          Decr Failure
        End If
      End If
    End Sub
    
    
    Measure:
     If Channel > 0 And Channel < 9 Then
       Kanal(channel) = Timer2
     End If
     Timer2 = 6
     Incr Channel
    Return
    
    
    Pausedetect:
      If Channel <> 0 Then
        Newvalsreceived = 1
      End If
      Channel = 0
    Return
    
    
    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()
    
     If Newvalsreceived = 1 Then
       Newvalsreceived = 0
    
       For I = 1 To Maxchannel
         Fkanal(i) = Fkanal(i) * 7                              '3
         Fkanal(i) = Fkanal(i) + Kanal(i)
         Shift Fkanal(i) , Right , 3                            '2
         Kanal(i) = Fkanal(i)
       Next I
    
       If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
         _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
         _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
         If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
         If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
       End If
    
       For I = 2 To Maxchannel
         If Kanal(i) >= 60 And Kanal(i) < 141 Then
           _sbl(i) = Kanal(i) - 100
           If _sbl(i) => -1 And _sbl(i) < 2 Then _sbl(i) = 0
           _sbl(i) = _sbl(i) * 6
         End If
       Next I
    
       If Kanal(5) < 80 Then State = 0
       If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
       If Kanal(5) >= 115 Then State = 2
    
       If State = 0 Or Oldstate <> State Then
          Rollstickold = 0
          Pitchstickold = 0
       End If
    
       Yawstickvel = _sbl(_yawchannel) + Yawstickold
       Yawstickold = _sbl(_yawchannel)
       Rollstickvel = _sbl(_rollchannel) + Rollstickold
       Rollstickold = _sbl(_rollchannel)
       Pitchstickvel = _sbl(_pitchchannel) + Pitchstickold
       Pitchstickold = _sbl(_pitchchannel)
    
       Yawstickvel = Yawstickvel / 4
       Yawstickvel = Yawstickvel * 3
    
       If State < 2 Then
         Rollstickvel = Rollstickvel / 10
         Pitchstickvel = Pitchstickvel / 10
       Else
         Rollstickvel = Rollstickvel / 4
         Pitchstickvel = Pitchstickvel / 4
       End If
    
    '(
    '#########P-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.085
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.085
    
       _roll_kp = 2.80 + _x1
       _pitch_kp = _roll_kp
    
    '#########I-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.0005
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.0005
    
       _roll_ki = 0.0050 + _x1
       _pitch_ki = _roll_ki
    
    '#########D-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.00002
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.00002
    
       _roll_kd = 0.0008 + _x1
       _pitch_kd = _roll_kd
    ')
    
    
       If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
         Call Set_wmp_offset()
       End If
    
     Oldstate = State
    
     End If
    
    End Sub
    
    
    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 , 4
       Shift Roll , Right , 4
       Shift Pitch , Right , 4
    End Sub
    
    
    Sub Set_wmp_offset()
       _yawoffset = 0
       _rolloffset = 0
       _pitchoffset = 0
       For I = 1 To 100
          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_int = Yawstickvel - _yawnow
     _yaw_err = _yaw_err_int
    
     _roll_err_int = Rollstickvel - _rollnow
     _roll_err = _roll_err_int
    
     _pitch_err_int = Pitchstickvel - _pitchnow
     _pitch_err = _pitch_err_int
    
    '##############PROPORTIONAL##############
    
     _yaw_kp_err = _yaw_err * _yaw_kp
    
     _roll_kp_err = _roll_err * _roll_kp
    
     _pitch_kp_err = _pitch_err * _pitch_kp
    
    '##############INTEGRAL##################
    
     _yaw_ki_err = _yaw_err * _yaw_ki
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _roll_ki_err = _roll_err * _roll_ki
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _pitch_ki_err = _pitch_err * _pitch_ki
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    '##############DIFFERENTIAL#############
    
     _yaw_kd_err = _yaw_err * _yaw_kd
     _yaw_kd_err = _yaw_kd_err - _yaw_kd_old
     _yaw_kd_old = _yaw_kd_err
    
     _roll_kd_err = _roll_err * _roll_kd
     _roll_kd_err = _roll_kd_err - _roll_kd_old
     _roll_kd_old = _roll_kd_err
    
     _pitch_kd_err = _pitch_err * _pitch_kd
     _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
     _yaw_pid_int = _yaw_pid
    
     _roll_pid = _roll_kp_err + _roll_ki_sum
     _roll_pid = _roll_pid + _roll_kd_err
     _roll_pid_int = _roll_pid
    
     _pitch_pid = _pitch_kp_err + _pitch_ki_sum
     _pitch_pid = _pitch_pid + _pitch_kd_err
     _pitch_pid_int = _pitch_pid
    
    '###############WERTE_BEGRENZEN##########
    
     If _yaw_pid_int < -68 Then _yaw_pid_int = -68
     If _yaw_pid_int > 68 Then _yaw_pid_int = 68
     If _roll_pid_int < -68 Then _roll_pid_int = -68
     If _roll_pid_int > 68 Then _roll_pid_int = 68
     If _pitch_pid_int < -68 Then _pitch_pid_int = -68
     If _pitch_pid_int > 68 Then _pitch_pid_int = 68
    
    End Sub
    
    
    Sub Mixer()
    
      '_bl(1) = rechts hinten  (US)
      '_bl(2) = links hinten   (GUS)
      '_bl(3) = rechts vorne   (GUS)
      '_bl(4) = links vorne    (US)
    
     _bl(1) = 182 + _sbl(_throttlechannel)
     _bl(2) = _bl(1)
     _bl(3) = _bl(1)
     _bl(4) = _bl(1)
    
     If State <> 0 And Failure < 20 Then
    
       Portc.2 = 0
       Portc.3 = 1
    
       _bl(1) = _bl(1) + Minthrottle
       _bl(2) = _bl(2) + Minthrottle
       _bl(3) = _bl(3) + Minthrottle
       _bl(4) = _bl(4) + Minthrottle
    
       _bl(1) = _bl(1) - _pitch_pid_int
       _bl(2) = _bl(2) - _pitch_pid_int
       _bl(3) = _bl(3) + _pitch_pid_int
       _bl(4) = _bl(4) + _pitch_pid_int
    
       _bl(1) = _bl(1) + _roll_pid_int
       _bl(2) = _bl(2) - _roll_pid_int
       _bl(3) = _bl(3) + _roll_pid_int
       _bl(4) = _bl(4) - _roll_pid_int
    
       _bl(1) = _bl(1) - _yaw_pid_int
       _bl(2) = _bl(2) + _yaw_pid_int
       _bl(3) = _bl(3) + _yaw_pid_int
       _bl(4) = _bl(4) - _yaw_pid_int
    
     Else
    
       Portc.2 = 0
    
       _bl(1) = 113
       _bl(2) = 113
       _bl(3) = 113
       _bl(4) = 113
    
       Incr _blink
       If _blink = 100 Then
         Portc.3 = 1
         _blink = 101
       End If
       If _blink = 200 Then
         Portc.3 = 0
         _blink = 0
       End If
    
       _yaw_kp_err = 0
       _yaw_ki_sum = 0
       _yaw_kd_err = 0
       _yaw_ki_err = 0
       _roll_kp_err = 0
       _roll_ki_sum = 0
       _roll_kd_err = 0
       _roll_ki_err = 0
       _pitch_kp_err = 0
       _pitch_ki_sum = 0
       _pitch_kd_err = 0
       _pitch_ki_err = 0
       _yaw_pid_int = 0
       _roll_pid_int = 0
       _pitch_pid_int = 0
       _yaw_pid = 0
       _roll_pid = 0
       _pitch_pid = 0
       Yawstickvel = 0
       Rollstickvel = 0
       Pitchstickvel = 0
       Yawstickold = 0
       Rollstickold = 0
       Pitchstickold = 0
    
     End If
    
    End Sub
    
    
    Sub Init_system()
    
    _bl(1) = 113
    _bl(2) = 113
    _bl(3) = 113
    _bl(4) = 113
    
    _sbl(1) = -68
    _sbl(2) = 0
    _sbl(3) = 0
    _sbl(4) = 0
    _sbl(5) = -600
    _sbl(6) = -600
    _sbl(7) = 0
    _sbl(8) = 0
    
    Fkanal(1) = 63
    Fkanal(2) = 100
    Fkanal(3) = 100
    Fkanal(4) = 100
    Fkanal(5) = 63
    Fkanal(6) = 63
    Fkanal(7) = 100
    Fkanal(8) = 100
    
    Reset Newvalsreceived
    
    State = 0
    Oldstate = 0
    
    _yawnow = 0
    _rollnow = 0
    _pitchnow = 0
    
    Call Wmp_init()
    Waitms 500
    For I = 1 To 10
    Call Read_wmp_data()
    Next I
    Waitms 500
    Call Set_wmp_offset()
    
    Rc_on = 0
    
    End Sub
    
    End
    Ist das ein Bug, oder bin ich einfach nur zu blöd, den Unterschied (falls vorhanden) zu bemerken?

    Vielen Dank & Gruß
    Chris

    EDIT:
    Habs gerade probiert, "Timer2" durch "TCNT2" zu ersetzen, bringt aber auch nichts... Werd jetzt mal schauen, ob die OVF-ISR (Pausedetect) überhaupt aufgerufen wird.
    Geändert von Che Guevara (24.09.2011 um 11:50 Uhr)

  2. #2
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544
    Da mein erster Post schon zu lange war, muss ich einen zweiten machen.
    Hab jetzt mal spaßeshalber den Timer0 (PWM) auskommentiert, dann funktionierts.. Hier der Code:
    Code:
    $regfile = "m328pdef.dat"
    $crystal = 16000000
    $framesize = 250
    $hwstack = 250
    $swstack = 250
    $baud = 19200
    
    
    Config Serialin = Buffered , Size = 50 , Bytematch = 13
    
    
    Declare Sub Serial0charmatch()
    
    Declare Sub Init_system()
    
    Declare Sub Filter_gyro_data()
    Declare Sub Filter_rx_data()
    Declare Sub Mixer()
    
    Declare Sub Pid_regulator()
    
    Declare Sub Wmp_init()
    Declare Sub Send_zero()
    Declare Sub Read_wmp_data()
    Declare Sub Set_wmp_offset()
    
    Declare Sub Failsave()
    
    Declare Sub Set_pwm()
    
    
    
    Config Portc.2 = Output
    Config Portc.3 = Output
    Portc.2 = 0
    Portc.3 = 0
    
    
    Config Portb.1 = Output
    Config Portb.2 = Output
    Config Portd.5 = Output
    Config Portd.6 = Output
    
    
    Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    Pwm1a = 113
    Pwm1b = 113
    
    'Config Timer0 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
    'Pwm0a = 113
    'Pwm0b = 113
    
    
    Config Pind.2 = Input
    Portd.2 = 0
    
    Config Int0 = Falling
    On Int0 Measure
    Enable Int0
    
    Config Timer2 = Timer , Prescale = 256
    On Timer2 Pausedetect
    Enable Timer2
    
    
    $lib "I2C_TWI.LBX"
    Config Scl = Portc.5
    Config Sda = Portc.4
    Config Twi = 100000
    I2cinit
    
    
    Const Minthrottle = 20
    Const Maxchannel = 8
    Const _throttlechannel = 1
    Const _rollchannel = 2
    Const _pitchchannel = 3
    Const _yawchannel = 4
    Const _statechannel = 5
    Const _datachannel = 6
    
    Dim I As Byte
    Dim J As Byte
    Dim Newvalsreceived As Bit
    Dim _blink As Byte
    
    Dim _yaw_kp As Single
    Dim _roll_kp As Single
    Dim _pitch_kp As Single
    
    Dim _yaw_ki As Single
    Dim _roll_ki As Single
    Dim _pitch_ki As Single
    
    Dim _yaw_kd As Single
    Dim _roll_kd As Single
    Dim _pitch_kd As Single
    
    _yaw_kp = 0.18                                              '0.180
    _roll_kp = 0.18
    _pitch_kp = 0.18
    
    _yaw_ki = 0
    _roll_ki = 0
    _pitch_ki = 0
    
    _yaw_kd = 0
    _roll_kd = 0
    _pitch_kd = 0
    
    
    '###################################
    '#########RC-EMPFÄNGER##############
    '###################################
    Dim Bufferbyte As Byte
    Dim Kanal(maxchannel) As Word
    Dim Fkanal(maxchannel) As Word
    Dim Channel As Byte
    Dim _bl(4) As Word
    Dim _sbl(maxchannel) As Integer
    Dim State As Byte
    Dim Oldstate As Byte
    '###################################
    '###################################
    '###################################
    
    
    '###################################
    '###########I2C-Inputs##############
    '###################################
    Dim Wmplus_buffer(6) As Byte
    Dim Ar(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 Single
    Dim _roll_kp_err As Single
    Dim _pitch_kp_err As Single
    
    Dim _yaw_ki_err As Single
    Dim _roll_ki_err As Single
    Dim _pitch_ki_err As Single
    
    Dim _yaw_ki_sum As Single
    Dim _roll_ki_sum As Single
    Dim _pitch_ki_sum As Single
    
    Dim _yaw_kd_err As Single
    Dim _roll_kd_err As Single
    Dim _pitch_kd_err As Single
    
    Dim _yaw_kd_old As Single
    Dim _roll_kd_old As Single
    Dim _pitch_kd_old As Single
    
    Dim _yaw_pid_int As Integer
    Dim _roll_pid_int As Integer
    Dim _pitch_pid_int As Integer
    
    Dim _yaw_pid As Single
    Dim _roll_pid As Single
    Dim _pitch_pid As Single
    
    Dim _yaw_err_int As Integer
    Dim _roll_err_int As Integer
    Dim _pitch_err_int As Integer
    
    Dim _yaw_err As Single
    Dim _roll_err As Single
    Dim _pitch_err As Single
    '#################################
    '#################################
    '#################################
    
    Dim Yawstickvel As Integer
    Dim Rollstickvel As Integer
    Dim Pitchstickvel As Integer
    
    Dim Yawstickold As Integer
    Dim Rollstickold As Integer
    Dim Pitchstickold As Integer
    
    Dim _x1 As Single
    Dim _x2 As Single
    
    Dim Rc_on As Word
    
    Dim Failure As Byte
    
    
    
    Call Init_system()
    Waitms 500
    Enable Interrupts
    Waitms 500
    
    
    
    Rc_test:
    If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
       If Rc_on < 65000 Then Incr Rc_on
    Else
       If Rc_on > 0 Then Decr Rc_on
    End If
    If Rc_on < 500 Then
       Goto Rc_test
    End If
    
    
    
    
    '_bl(1) = rechts hinten  (US)
    '_bl(2) = links hinten   (GUS)
    '_bl(3) = rechts vorne   (GUS)
    '_bl(4) = links vorne    (US)
    
    'PWM = 113 --> 0.900us
    'PWM = 250 --> 2.000ms
    
    
    
    
    Do
    
    
       Call Filter_rx_data()
       Call Filter_gyro_data()
       Call Pid_regulator()
       Call Mixer()
       Call Failsave()
       Call Set_pwm()
    
    
    Loop
    
    
    Sub Serial0charmatch()
       Clear Serialin
    End Sub
    
    
    Sub Set_pwm()
       If _bl(1) > 250 Then _bl(1) = 250
       If _bl(1) < 113 Then _bl(1) = 113
       If _bl(2) > 250 Then _bl(2) = 250
       If _bl(2) < 113 Then _bl(2) = 113
       If _bl(3) > 250 Then _bl(3) = 250
       If _bl(3) < 113 Then _bl(3) = 113
       If _bl(4) > 250 Then _bl(4) = 250
       If _bl(4) < 113 Then _bl(4) = 113
       Pwm0a = _bl(4)                                           'links vorne (US)
       Pwm0b = _bl(2)                                           'links hinten (GUS)
       Pwm1a = _bl(3)                                           'rechts vorne (GUS)
       Pwm1b = _bl(1)                                           'rechts hinten (US)
       If Kanal(_datachannel) > 100 Then
         Print Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5)
       End If
    End Sub
    
    
    Sub Failsave()
      If Channel >= 11 Then
        If Failure < 255 Then
          Incr Failure
        End If
      End If
      If Channel < 11 Then
        If Failure > 0 Then
          Decr Failure
        End If
      End If
    End Sub
    
    
    Measure:
     If Channel > 0 And Channel < 9 Then
       Kanal(channel) = Timer2
     End If
     Timer2 = 6
     Incr Channel
    Return
    
    
    Pausedetect:
      If Channel <> 0 Then
        Newvalsreceived = 1
      End If
      Channel = 0
    Return
    
    
    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()
    
     If Newvalsreceived = 1 Then
       Newvalsreceived = 0
    
       For I = 1 To Maxchannel
         Fkanal(i) = Fkanal(i) * 7                              '3
         Fkanal(i) = Fkanal(i) + Kanal(i)
         Shift Fkanal(i) , Right , 3                            '2
         Kanal(i) = Fkanal(i)
       Next I
    
       If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
         _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
         _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
         If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
         If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
       End If
    
       For I = 2 To Maxchannel
         If Kanal(i) >= 60 And Kanal(i) < 141 Then
           _sbl(i) = Kanal(i) - 100
           If _sbl(i) => -1 And _sbl(i) < 2 Then _sbl(i) = 0
           _sbl(i) = _sbl(i) * 6
         End If
       Next I
    
       If Kanal(5) < 80 Then State = 0
       If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
       If Kanal(5) >= 115 Then State = 2
    
       If State = 0 Or Oldstate <> State Then
          Rollstickold = 0
          Pitchstickold = 0
       End If
    
       Yawstickvel = _sbl(_yawchannel) + Yawstickold
       Yawstickold = _sbl(_yawchannel)
       Rollstickvel = _sbl(_rollchannel) + Rollstickold
       Rollstickold = _sbl(_rollchannel)
       Pitchstickvel = _sbl(_pitchchannel) + Pitchstickold
       Pitchstickold = _sbl(_pitchchannel)
    
       Yawstickvel = Yawstickvel / 4
       Yawstickvel = Yawstickvel * 3
    
       If State < 2 Then
         Rollstickvel = Rollstickvel / 10
         Pitchstickvel = Pitchstickvel / 10
       Else
         Rollstickvel = Rollstickvel / 4
         Pitchstickvel = Pitchstickvel / 4
       End If
    
    '(
    '#########P-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.085
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.085
    
       _roll_kp = 2.80 + _x1
       _pitch_kp = _roll_kp
    
    '#########I-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.0005
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.0005
    
       _roll_ki = 0.0050 + _x1
       _pitch_ki = _roll_ki
    
    '#########D-GAIN###########
       _x1 = Kanal(7) - 100
       _x1 = _x1 * 0.00002
       _x2 = Kanal(8) - 100
       _x2 = _x2 * 0.00002
    
       _roll_kd = 0.0008 + _x1
       _pitch_kd = _roll_kd
    ')
    
    
       If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
         Call Set_wmp_offset()
       End If
    
     Oldstate = State
    
     End If
    
    End Sub
    
    
    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 , 4
       Shift Roll , Right , 4
       Shift Pitch , Right , 4
    End Sub
    
    
    Sub Set_wmp_offset()
       _yawoffset = 0
       _rolloffset = 0
       _pitchoffset = 0
       For I = 1 To 100
          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_int = Yawstickvel - _yawnow
     _yaw_err = _yaw_err_int
    
     _roll_err_int = Rollstickvel - _rollnow
     _roll_err = _roll_err_int
    
     _pitch_err_int = Pitchstickvel - _pitchnow
     _pitch_err = _pitch_err_int
    
    '##############PROPORTIONAL##############
    
     _yaw_kp_err = _yaw_err * _yaw_kp
    
     _roll_kp_err = _roll_err * _roll_kp
    
     _pitch_kp_err = _pitch_err * _pitch_kp
    
    '##############INTEGRAL##################
    
     _yaw_ki_err = _yaw_err * _yaw_ki
     _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
    
     _roll_ki_err = _roll_err * _roll_ki
     _roll_ki_sum = _roll_ki_sum + _roll_ki_err
    
     _pitch_ki_err = _pitch_err * _pitch_ki
     _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
    
    '##############DIFFERENTIAL#############
    
     _yaw_kd_err = _yaw_err * _yaw_kd
     _yaw_kd_err = _yaw_kd_err - _yaw_kd_old
     _yaw_kd_old = _yaw_kd_err
    
     _roll_kd_err = _roll_err * _roll_kd
     _roll_kd_err = _roll_kd_err - _roll_kd_old
     _roll_kd_old = _roll_kd_err
    
     _pitch_kd_err = _pitch_err * _pitch_kd
     _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
     _yaw_pid_int = _yaw_pid
    
     _roll_pid = _roll_kp_err + _roll_ki_sum
     _roll_pid = _roll_pid + _roll_kd_err
     _roll_pid_int = _roll_pid
    
     _pitch_pid = _pitch_kp_err + _pitch_ki_sum
     _pitch_pid = _pitch_pid + _pitch_kd_err
     _pitch_pid_int = _pitch_pid
    
    '###############WERTE_BEGRENZEN##########
    
     If _yaw_pid_int < -68 Then _yaw_pid_int = -68
     If _yaw_pid_int > 68 Then _yaw_pid_int = 68
     If _roll_pid_int < -68 Then _roll_pid_int = -68
     If _roll_pid_int > 68 Then _roll_pid_int = 68
     If _pitch_pid_int < -68 Then _pitch_pid_int = -68
     If _pitch_pid_int > 68 Then _pitch_pid_int = 68
    
    End Sub
    
    
    Sub Mixer()
    
      '_bl(1) = rechts hinten  (US)
      '_bl(2) = links hinten   (GUS)
      '_bl(3) = rechts vorne   (GUS)
      '_bl(4) = links vorne    (US)
    
     _bl(1) = 182 + _sbl(_throttlechannel)
     _bl(2) = _bl(1)
     _bl(3) = _bl(1)
     _bl(4) = _bl(1)
    
     If State <> 0 And Failure < 20 Then
    
       Portc.2 = 0
       Portc.3 = 1
    
       _bl(1) = _bl(1) + Minthrottle
       _bl(2) = _bl(2) + Minthrottle
       _bl(3) = _bl(3) + Minthrottle
       _bl(4) = _bl(4) + Minthrottle
    
       _bl(1) = _bl(1) - _pitch_pid_int
       _bl(2) = _bl(2) - _pitch_pid_int
       _bl(3) = _bl(3) + _pitch_pid_int
       _bl(4) = _bl(4) + _pitch_pid_int
    
       _bl(1) = _bl(1) + _roll_pid_int
       _bl(2) = _bl(2) - _roll_pid_int
       _bl(3) = _bl(3) + _roll_pid_int
       _bl(4) = _bl(4) - _roll_pid_int
    
       _bl(1) = _bl(1) - _yaw_pid_int
       _bl(2) = _bl(2) + _yaw_pid_int
       _bl(3) = _bl(3) + _yaw_pid_int
       _bl(4) = _bl(4) - _yaw_pid_int
    
     Else
    
       Portc.2 = 0
    
       _bl(1) = 113
       _bl(2) = 113
       _bl(3) = 113
       _bl(4) = 113
    
       Incr _blink
       If _blink = 100 Then
         Portc.3 = 1
         _blink = 101
       End If
       If _blink = 200 Then
         Portc.3 = 0
         _blink = 0
       End If
    
       _yaw_kp_err = 0
       _yaw_ki_sum = 0
       _yaw_kd_err = 0
       _yaw_ki_err = 0
       _roll_kp_err = 0
       _roll_ki_sum = 0
       _roll_kd_err = 0
       _roll_ki_err = 0
       _pitch_kp_err = 0
       _pitch_ki_sum = 0
       _pitch_kd_err = 0
       _pitch_ki_err = 0
       _yaw_pid_int = 0
       _roll_pid_int = 0
       _pitch_pid_int = 0
       _yaw_pid = 0
       _roll_pid = 0
       _pitch_pid = 0
       Yawstickvel = 0
       Rollstickvel = 0
       Pitchstickvel = 0
       Yawstickold = 0
       Rollstickold = 0
       Pitchstickold = 0
    
     End If
    
    End Sub
    
    
    Sub Init_system()
    
    _bl(1) = 113
    _bl(2) = 113
    _bl(3) = 113
    _bl(4) = 113
    
    _sbl(1) = -68
    _sbl(2) = 0
    _sbl(3) = 0
    _sbl(4) = 0
    _sbl(5) = -600
    _sbl(6) = -600
    _sbl(7) = 0
    _sbl(8) = 0
    
    Fkanal(1) = 63
    Fkanal(2) = 100
    Fkanal(3) = 100
    Fkanal(4) = 100
    Fkanal(5) = 63
    Fkanal(6) = 63
    Fkanal(7) = 100
    Fkanal(8) = 100
    
    Reset Newvalsreceived
    
    State = 0
    Oldstate = 0
    
    _yawnow = 0
    _rollnow = 0
    _pitchnow = 0
    
    Call Wmp_init()
    Waitms 500
    For I = 1 To 10
    Call Read_wmp_data()
    Next I
    Waitms 500
    Call Set_wmp_offset()
    
    Rc_on = 0
    
    End Sub
    
    End
    Das finde ich sehr merkwürdig! Falls jemand ein paar Tips / Anregungen / sonstiges hat, immer her damit

  3. #3
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    22.06.2009
    Beiträge
    1.265
    Du könntest 1. Mal im Datenblatt nachschaun ob sich die beiden Timer irgendwie beeinflussen.
    2. Versuchen die Register direkt zusetzen (Also das jeweils richtige Bit im TCNT2 Register und soweiter). Die Register und die zusetzenden Bits findest du auch im Datenblatt.

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

    im DB habe ich mir schon die meisten Dinge zu den Timern durchgelesen, habe jedoch nichts relevantes gesehen! Hast du einen Tip, wo ich nachsehen sollte?
    Morgen Abend werde ich mal versuchen, die Timer über die Register zu konfigurieren. Mal sehen...
    Da ich unbedingt fliegen wollte, habe ich heute kurzerhand die 2 Verbindungen vom uC zu den ESC unterbrochen und jeweils ein Kabel direkt vom uC zum ESC verlegt. Jetzt verwende ich den ersten Code, d.h. Timer0 liest das Signal ein und Timer2 generiert die PWMs. Trotzdem möchte ich gerne wissen, warums anders rum nicht funktioniert!

    Gruß & Vielen Dank
    Chris

  5. #5
    Erfahrener Benutzer Roboter-Spezialist
    Registriert seit
    01.10.2009
    Beiträge
    437
    Zitat Zitat von Che Guevara Beitrag anzeigen
    Trotzdem möchte ich gerne wissen, warums anders rum nicht funktioniert!
    Die Register sind richtig gesetzt, Timer laufen also und Interrupts werden auch ausgelöst, und das in beiden Versionen, im Simulator ist kein Fehler zu entdecken. Ich würde ein HW-Problem vermuten, denn Timer0 hat keine Auswirkung auf Timer2.

    Ist denn gewährleistet, daß die PWM von Timer0 ok ist ? Um systematisch vorzugehen würde man erst einmal die Pins PD5..6 in einer Delayloop toggeln. Wenn das geht, den nichtfunktionierenden Code auf das absolute Minimum reduzieren, also Config der Timer, und einen freien Pin in der Pausedetect-ISR toggeln, um auch deren Funktion zu überprüfen.

    Wenn ein Oszi vorhanden ist, gut, wenn nicht, dann zeigen normale Multimeter üblicherweise eine Spannung entsprechend des Taktgrades an.

    Ab Bascom 2.0.7.1 ist übrigens die Wirkung von ... = Clear Down genau gegenläufig und entspricht nun den Angaben in den Atmel-Datenblättern. Da es jedoch sehr unwahrscheinlich ist, daß die gezeigten Codes mit unterschiedlichen Versionen compiliert wurden, sollte das nicht das Problem sein.

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

    PWM funktioniert von allen 3 Timern richtig! Oszi ist vorhanden, mit diesem habe ich auch schon die Funktionstüchtigkeit der PWMs überprüft.
    Ich habe leider momentan keine Vermutung, wo das Problem liegen könnte. Werd aber heute Abend mal ein paar Tests durchführen.

    Gruß
    Chris

  7. #7
    Erfahrener Benutzer Roboter-Spezialist
    Registriert seit
    01.10.2009
    Beiträge
    437
    Zitat Zitat von Che Guevara Beitrag anzeigen
    PWM funktioniert von allen 3 Timern richtig! Oszi ist vorhanden, mit diesem habe ich auch schon die Funktionstüchtigkeit der PWMs überprüft.
    Nachdem ich die Timer alle 3 im AVR-Studio laufen sehe (zum Testen die Prescaler verkleinert) und auch der Interrupt auf Pausedetect richtig kommt, bleiben nicht so viele Möglichkeiten über.

    Voraussetzung ist, daß der nichtfunktionierende Code gegenüber dem Funktionierenden nur im Bereich der Timer-Config, bzw. Zuweisung der Arrayelemente _bl 4,2 verändert wurde und sich keine weiteren versteckten Zugriffe auf den Timer im Code versteckt halten.
    Fand beim Drübersehen zumindest keine.

    Eine Fehlermöglichkeit wäre sicher eine ältere Bascom-Version, die Timer2 nicht richtig setzt. Was für eine Version wird genutzt ? Ich kann an der erzeugte .hex-Datei erkennen, ob da was falsch gesetzt wird, kannst ja mal anhängen.

  8. #8
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544
    Die Codes sind bis auf die Timer-Configs komplett gleich. Die Timer-Anweisungen kommen nur in Pausedetect und in Measure vor.
    Bascom Version ist die 1.11.9.5. Hex File ist im Anhand, danke

    Gruß
    Chris
    Angehängte Dateien Angehängte Dateien

  9. #9
    Erfahrener Benutzer Roboter-Spezialist
    Registriert seit
    01.10.2009
    Beiträge
    437
    Also Timer2 in dem Hex läuft, die Register sind ordentlich gesetzt und die ISR wird aufgerufen, wenn ich das I-Flag im SREG manuell setze. Bis zum Enable Interrupts geht die Simulation nicht, denn die Waits blockieren, später auch noch das I2C, ohne I2C-HW geht da nicht viel.

    Ich kann mir immer noch vorstellen, daß es ein HW-Problem ist, denn die Timer beeinflussen sich intern nicht, außer einer schreibt dem anderen die Register kaputt, da muss aber zusätzlicher Code im Spiel sein. Kommentier' doch mal den Timer0 als PWM aus und setz' OC0A..B auf Ausgang und 1, oder toggle OC0A..B in der Mainloop.

    Anbei ein per 2.0.7.0 kompiliertes Hex, sollte das laufen, wird's Zeit für ein Update.
    Angehängte Dateien Angehängte Dateien

  10. #10
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544
    Erstmal danke fürs drüberschauen Aber eine Frage hätte ich: Wie kannst du überprüfen, ob das Hex file korrekt ist?
    Ein HW-Problem kann ich ausschließen, den wenn ich Timer0 (als PWM) auskommentiere, funzt Timer2 ... PWM von Timer0 funzt auch, wenn ich z.b. fest vorgebene Werte in die Register schreibe (PWM0a und PWM0b) werden diese korrekt ausgegeben (mit dem Oszi und Motoren überprüft). Lediglich beide Timer zusammen (Timer0 als PWM; Timer2 fürs Summensignal) funktionieren nicht.
    Das Hex File werde ich morgen Abend mal draufspielen, werde dann berichten, obs geklappt hat. Vielen Dank schonmal.

    Gruß
    Chris

Seite 1 von 2 12 LetzteLetzte

Ähnliche Themen

  1. PWM mit ATMEGA644p mit timer0, timer1 und timer2
    Von lebratmaxe im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 4
    Letzter Beitrag: 29.07.2010, 19:42
  2. Atmega128, Timer2 als PWM mag nicht
    Von Felix H. im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 2
    Letzter Beitrag: 16.07.2010, 14:35
  3. Timer2 (Uhrenquarz) und Timer0 PWM vertragen sich nicht
    Von Marten83 im Forum C - Programmierung (GCC u.a.)
    Antworten: 2
    Letzter Beitrag: 23.04.2009, 13:41
  4. Timer0 oder Timer2 benutzen, aber wie?
    Von H3llGhost im Forum Asuro
    Antworten: 17
    Letzter Beitrag: 08.01.2008, 18:15
  5. Timer2 overflow Interrupt will nicht
    Von BomberD im Forum C - Programmierung (GCC u.a.)
    Antworten: 10
    Letzter Beitrag: 30.01.2006, 17:37

Berechtigungen

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