Projekt Mähroboter

[Feuerring]

Update ...

bin einen guten Schritt weiter gekommen ...

Das Antriebs-Modul ist weitgehend fertig, Das Modul wird wie alle anderen Module dann auf eine BUS-Platine gesteckt,
dadurch kann ich flexibel auch spätere Erweiterungen nachrüsten.



Meine Test-Umgebung:


Das Modul steuere ich derzeit über einen IO-Warrior (I2C) und dem Programm All-In-One,
zur Auswertung von Parametern nehme ich die RS232-Schnittstelle und einem einfachen eigenen PC-Programm.

Das Antriebs-Modul steuert die Motoren über Treiber-Module per PWM/DIR/Brake,
die Drehzahl und Drehrichtung der beiden Motore wird über die Tacho-Einheit ausgewertet.

Dank der Hilfe im Bascom-Forum, konnte ich recht schnell die TWI-Hardware-Lösung ohne LIB umsetzen ... siehe hier

Das Modul wird wie folgt über TWI(I2C) angesprochen

schreiben:
ADR|Mode|W1|W2|Wn
lesen:
ADR|Mode|R1|R2|Rn

Funktionen
Mode=0 ( schnelles bremsen ) ... funktioniert sicher
schreiben:
ADR|00 ... beide Motoren fahren schnell Ihre Drehzahl runter
lesen:
ADR|00|FF ... keine Daten außer Mode, Rest wir mit FF gefüllt

Mode=1 ( Speed ) ... funktioniert sicher
schreiben:
ADR|01|DR|V0|V1 ... beide Motoren werden beschleunigt bzw. abgebremst um auf die vorgegeben Drehzahl zu kommen und halten diese konstant.
lesen:
ADR|01|DR|V0|V1|P0|P1
DR = Drehrichtung 1= Motor_0 rückwärts / 2=Motor_1 rückwärts / 0= vorwärts beide / 3= rückwärts beide ... kann nur bei stehenden Motoren geändert werden
V0 = Drehzahl-Vorgabe Motor_0 0=Null / 150 = 7500 U/min ( Werte für die Faulhaber-Motoren)
V1 = Drehzahl-Vorgabe Motor_1 0=Null / 150 = 7500 U/min ( Werte für die Faulhaber-Motoren)
P0 = PWM-Wert Motor_0 ( nur lesen )
P1 = PWM-Wert Motor_1 ( nur lesen )

Mode=2 ( Strecke) .... noch nicht implementiert ... eine Wegstrecke wird mit Vmax gefahren ... auch Kurven oder drehen ist möglich.
schreiben:
ADR|02|DR|VM|S0|S0|S1|S1
lesen:
ADR|02|DR|VM|V0|V1|P0|P1

DR = Drehrichtung 1= Motor_0 rückwärts / 2=Motor_1 rückwärts / 0= vorwärts beide / 3= rückwärts beide
VM = Vmax
S0 = Strecke für Motor_0
S1 = Strecke für Motor_1

.....
zischen den Mode-1 und Mode-2 kann nicht direkt umgeschaltet werden nur über Mode-0 ... je nach Mode kann auch nur die dafür vorgegeben Werte gelesen werden.

ist das Modul am beschleunigen oder bremsen oder fährt die vorgegeben Wegstrecke wird die Leitung Ready auf LOW gelegt,
so muss der Master nicht immer über I2C pollen.

Wird die NOT-AUS-Leitung (BUS) auf LOW gelegt werden die Motoren über die Gatter sofort deaktiviert und das Modul fährt die Werte intern runter.

-----

derzeitiger Code:

'  Projekt Mäh-Roboter
'  R. Greinert
'  (CC) 07 / 2016 
'
'  Antrieb_Steuerung  Ver.008
'
'
' ------------ Bascom - Parameter ------------------------
$regfile = "m328pdef.dat"
$crystal = 16000000
$hwstack = 100
$swstack = 100
$framesize = 100
$baud = 19200


Print "Start_Main"


' ------------ Timer_0 Zeitvorgabe ------------------------------
Config Timer0 = Timer , Prescale = 256
On Ovf0 Timer0_ovf
Enable Timer0


' ------------ Output ------------------------------
Config Portb.0 = Output                                     ' Ready-Leitung
Config Portb.3 = Output                                     ' DIR Motor_0
Config Portb.4 = Output                                     ' DIR Motor_1
Config Portb.5 = Output                                     ' Herzschlag
Config Portd.4 = Output                                     ' Break Motor_0
Config Portd.5 = Output                                     ' Break Motor_1
Config Portd.7 = Output                                     ' I2C-Data-Anzeige


Portb.0 = 0                                                 ' NOT-Ready = LOW
Portb.3 = 0                                                 ' DIR Motor_0   0=vorwärts 1=Rückwärts
Portb.4 = 0                                                 ' DIR Motor_1   0=vorwärts 1=Rückwärts
Portd.4 = 0                                                 ' Break Motor_0 low-aktiv
Portd.5 = 0                                                 ' Break Motor_1 low-aktiv
Portd.7 = 1                                                 ' I2C-LED aus


' ------------ Input ------------------------------
Config Pinc.2 = Input                                       ' Motor_0 Decoder Q2
Config Pinc.3 = Input                                       ' Motor_1 Decoder Q2
Config Pinc.0 = Input                                       ' Abfrage NOT-AUS LOW-Aktiv
Config Pinc.1 = Input                                       ' Abfrage RS232-Enabled


' ------------ PWM  ----------------------
Config Portb.1 = Output                                     ' PWM Motor_0
Config Portb.2 = Output                                     ' PWM Motor_1
Portb.1 = 0
Portb.2 = 0
Config Timer1 = Pwm , Pwm = 8 , Compare_a_pwm = Clear_up , Compare_b_pwm = Clear_up , Prescale = 64


' ------------ RS232 - Input ------------------------
Config Serialin = Buffered , Size = 40


' ------------ I2C-Slave  ------------------------
Dim Twi_status As Byte
Dim Twi_control As Byte
Dim Twi_dn As Byte


Twsr = 0
Twdr = 255
Twar = 180
Twcr = &B11000100


' ------------ DIM ------------------------
Dim Na As String * 30                                       ' RS232 Input-String
Dim Nax As String * 30                                      ' RS232 Input-UCASE-String
Dim Si As Byte                                              ' RS232 Input Zeichen
Dim Z0 As Integer                                           ' Zähler Motor_0
Dim Z1 As Integer                                           ' Zähler Motor_1
Dim Z0x As Word                                             ' Zähler cache Motor_0
Dim Z1x As Word                                             ' Zähler cache Motor_1
Dim Tx As Byte                                              ' Zähler ms
Dim Tmx As Byte                                             ' Zähler sec
Dim P0 As Byte                                              ' PWM_Motor_0 Stellwert
Dim P1 As Byte                                              ' PWM_Motor_1 Stellwert
Dim Px As Byte                                              ' Temp-Wert Stellwert_diff soll
Dim Pt As Word                                              ' Temp-wert Stellwert_diff Ist
Dim Vx As Integer                                           ' Temp-Wert Drehzahl_Diff
Dim V0 As Byte                                              ' Vorgabe Drehzahl Motor_0
Dim V1 As Byte                                              ' Vorgabe Drehzahl Motor_1
Dim R_0 As Byte                                             ' Vorgabe Richtung Motor_0
Dim R_1 As Byte                                             ' Vorgabe Richtung Motor_1
Dim Sx As Boolean                                           ' Ready 1=fertig 0=atwork
Dim S0 As Long                                              ' Streckenzähler Motor_0
Dim S1 As Long                                              ' Streckenzähler Motor_1
Dim Xmode As Byte                                           ' Funktion-Mode 0=Stop 1=Speed 2=Strecke
Dim Xt1 As Byte                                             ' temp1
' ------ Split -------
Dim Sb As Byte                                              ' Split_Zähler
Dim Sbx(5) As String * 10                                   ' Split_Array


' -------------- Declare Sub ----------------




' -------------- Interupt-Eingänge ----------------
Config Int0 = Falling                                       ' Motor_0 Decoder Q1
Config Int1 = Falling                                       ' Motor_1 Decoder Q1
Enable Int0
Enable Int1
On Int0 Int0_sub
On Int1 Int1_sub


Enable Interrupts


Print "Start_Hauptschleife"
Do
    ' --------------- Hauptschleife ------------------
    If Ischarwaiting() <> 0 Then
      Si = Inkey()
      If Pinc.1 = 0 Then                                    ' abfrage ob RS232 enabled
         If Si <> 13 And Si <> 10 And Len(na) < 30 Then
            Na = Na + Chr(si)
         Else
            Nax = Ucase(na)
            Sb = Split(nax , Sbx(1) , "|")
            Print Sb
            If Nax = "STATUS" Then
               Print "ACK|STATUS"
               Print "Status|Z0x=" ; Z0x ; "|Z1x=" ; Z1x


            Elseif Nax = "STOP" Then
               Xmode = 0
               Print "ACK|STOP"
            Elseif Left(nax , 5) = "SPEED" Then
               If Sb = 5 Then
                  Print "ACK|SPEED"
                  Xmode = 1
                  R_0 = Val(sbx(2) )
                  V0 = Val(sbx(3) )
                  R_1 = Val(sbx(4) )
                  V1 = Val(sbx(5) )
               Else
                   Print "NAK|" ; Na
               End If
            Else
               Print "NAK|" ; Na
            End If
            Na = ""
         End If
      End If
   End If


   ' ----------------- TWI ------------------
   Twi_control = Twcr And &H80                              ' Bit7 von Controlregister
   If Twi_control = &H80 Then
      ' TWI Transfer
      Portd.7 = 0
      Twi_status = Twsr




         If Twi_status = &H80 Or Twi_status = &H88 Then
            ' -------- Daten von TWI erhalten
            If Twi_dn = 0 Then
               If Twdr = 1 Then
                  Xmode = 1
               Elseif Twdr = 2 Then
                  Xmode = 2
               Else
                  Xmode = 0
               End If
            Elseif Twi_dn = 1 Then
                If Xmode = 1 Then
                  ' Richtung
                  Xt1 = Twdr And 1
                  R_0 = Xt1
                  Xt1 = Twdr And 2
                  If Xt1 = 2 Then
                     R_1 = 1
                  Else
                     R_1 = 0
                  End If
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 2 Then
               If Xmode = 1 Then
                  V0 = Twdr
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 3 Then
               If Xmode = 1 Then
                  V1 = Twdr
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 4 Then


            Elseif Twi_dn = 5 Then


            End If
            Incr Twi_dn
         Elseif Twi_status = &HB0 Or Twi_status = &HB8 Then
            ' ------- Daten an TWI übergeben
            If Xmode = 0 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Else
                  Twdr = 255
               End If
            Elseif Xmode = 1 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Elseif Twi_dn = 1 Then
                  Xt1 = 0
                  If R_0 = 1 Then Xt1 = 1
                  If R_1 = 1 Then Xt1 = Xt1 + 2
                  Twdr = Xt1
               Elseif Twi_dn = 2 Then
                  Twdr = V0
               Elseif Twi_dn = 3 Then
                  Twdr = V1
               Elseif Twi_dn = 4 Then
                  Twdr = P0
               Elseif Twi_dn = 5 Then
                  Twdr = P1
               Else
                  Twdr = 0
               End If
            Elseif Xmode = 2 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Else
                  Twdr = 255
               End If
            Else
               Twdr = 0
            End If


            Incr Twi_dn
         Elseif Twi_status = &HA0 Or Twi_status = &HC0 Then
            ' TWI Transfer beendet
            Portd.7 = 1
            Twi_dn = 0
         End If
         Twcr = &B11000100
   End If
Loop


Timer0_ovf:
   ' ---- Timer_0 ----- Interrupt alle 1 mS ----------------
   Timer0 = 194                                             ' Resrwert = 61
   Incr Tx
   If Tx = 80 Then Portb.5 = 0                              ' Herzschlag-LED (ein)


   If Tx > 99 Then
      ' -------- Zeitbasis 100 mS ------------------
      Sx = 1                                                ' Ready-Leitung =1
      Tx = 0
      Incr Tmx
      Z0x = Z0
      Z0 = 0
      Z1x = Z1
      Z1 = 0


      If P0 = 0 And P1 = 0 Then
         ' ------ Richtung setzen wenn Motoren stehen
         If R_0 = 0 Then Portb.3 = 0 Else Portb.3 = 1
         If R_1 = 0 Then Portb.4 = 0 Else Portb.4 = 1
      End If


      If Xmode = 0 Then
         ' ------ Mode - Stop ----------
         If P0 > 30 Then
            P0 = P0 - 30
            Sx = 0
         Else
            P0 = 0


         End If
         If P1 > 30 Then
            P1 = P1 - 30
            Sx = 0
         Else
            P1 = 0
         End If
      Elseif Xmode = 1 Then
         ' ------ Mode - Speed ----------
         If V0 > Z0x And P0 < 255 And Pinc.0 = 1 Then
            Px = 1
            Vx = V0 - Z0x
            If Vx > 6 Then
               Px = 5
               Sx = 0
            End If
            If Vx > 30 Then
               Px = 20
            End If
            Pt = P0 + Px
            If Pt > 255 Then
               P0 = 255
            Else
               P0 = P0 + Px
            End If
         End If


         If V1 > Z1x And P1 < 255 And Pinc.0 = 1 Then
            Px = 1
            Vx = V1 - Z1x
            If Vx > 6 Then
               Px = 5
               Sx = 0                                       ' Ready-Leitung =0
            End If
            If Vx > 30 Then
               Px = 20
            End If
            Pt = P1 + Px
            If Pt > 255 Then
               P1 = 255
            Else
               P1 = P1 + Px
            End If
         End If


         If V0 < Z0x And P0 > 0 Or Pinc.0 = 0 Then
            Px = 1
            Vx = Z0x - V0
            If Vx > 3 Then
               Px = 6
               Sx = 0
            End If                                          ' Ready-Leitung =0
            If Vx > 35 Then
               Px = 30
            End If
            If Px > P0 Then
               P0 = 0
            Else
               P0 = P0 - Px
            End If
         End If


         If V1 < Z1x And P1 > 0 Or Pinc.0 = 0 Then
            Px = 1
            Vx = Z1x - V1
            If Vx > 3 Then
               Px = 6
               Sx = 0
            End If                                          ' Ready-Leitung =0
            If Vx > 35 Then
               Px = 30
            End If
            If Px > P1 Then
               P1 = 0
            Else
               P1 = P1 - Px
            End If
         End If
      Elseif Xmode = 2 Then
         ' ---- Mode-Strecke ------------------
      End If


      ' ------ NOT - STOP ---- und Treiber auf null setzen ----
      If V0 = 0 And Z0x = 0 And P0 > 0 Or Pinc.0 = 0 Then P0 = 0
      If V1 = 0 And Z1x = 0 And P1 > 0 Or Pinc.0 = 0 Then P1 = 0


      ' ------ Treiber-Bremse -------
      If P0 > 0 Then
         Portd.4 = 1
      Else
         Portd.4 = 0
      End If


      If P1 > 0 Then
         Portd.5 = 1
      Else
         Portd.5 = 0
      End If


      ' ----- PWM-Wert setzen ---------
      Compare1a = P0
      Compare1b = P1


      Portb.0 = Sx                                          ' Ausgabe Ready-Leitung
   End If


   If Tmx > 9 Then
      ' -------------  Zeitbasis 1 Sec -------------------
      Tmx = 0
      If Pinc.1 = 0 Then                                    ' abfrage ob RS232 enabled
         Print "Status XM=" ; Xmode ; " Z0x=" ; Z0x ; " Z1x=" ; Z1x ; " P0=" ; P0 ; " P1=" ; P1 ; " S0=" ; S0 ; " S1=" ; S1 ; " NA=" ; Pinc.0
      End If
      Portb.5 = 1                                           ' Herzschlag-LED (aus)


   End If
Return
Return


Int0_sub:                                                   ' Interrupt Motor_0 Decoder_Q1
   Incr Z0
   If Pinc.2 = 0 Then                                       ' Abfrage Motor_0 Decoder_Q2 ( Drehrichtung )
      Incr S0
   Else
      Decr S0
   End If
Return
Return


Int1_sub:                                                   ' Interrupt Motor_1 Decoder_Q1
   Incr Z1
   If Pinc.3 = 0 Then                                       ' Abfrage Motor_1 Decoder_Q2 ( Drehrichtung )
      Incr S1
   Else
      Decr S1
   End If
Return
Return