'--------------------------------------------------------------
'                   FocuserRev2
'                  Robofocus Protokoll
'              (c) 2005-2007 Thomas Westerhoff
' Version mit endlagenschaltern an Port
'--------------------------------------------------------------
$loadersize = 512
$regfile = "M8def.dat"
$crystal = 6000000
$baud = 19200
$hwstack = 128
$swstack = 128
$framesize = 32




Config Serialin = Buffered , Size = 20
Config Portd = Input
Config Portc = Output
Config Int0 = Falling
Config Int1 = Falling


' Timer0 für die Schrittimpulse definieren
Config Timer0 = Timer , Prescale = 1024                     'Timer auf 0,17ms einstellen   für RS232

Config 1wire = Portc.0                                      'Pin für den TemperaturSensor
On Timer0 Tim0_isr


' Ports für die Motoransteuzerung
Stepperenable Alias Portc.1                                 ' PortC.1     Enable           Enable
'Clock Alias Pinc.2                                          ' PortC.2     Takt             Clock
Dirout Alias Portc.3                                        ' PortC.3     Richtung         CW/CCW                 0 = in, 1 =out
Stepperreset Alias Portc.4                                  ' PORTC.4     Reset            RESET
Halfstep Alias Portc.5                                      ' PortC.5     Geschwindigkeit  H/F

Dim Inpuffer As String * 10
Dim Zeichen As String * 1
Dim Tempstr As String * 10
Dim Ds1820 As Integer
Dim Temp As String * 10
Dim Temp1 As String * 10
Dim Gotochar As String * 1
Dim Schecksum As String * 1

Dim I As Integer
Dim Count As Integer
Dim Value As Integer                                        'Speicher für die zu tätigende Schrittanzahl
Dim Actualposition As Integer                               'Speicher für aktuelle Position
Dim Actualtemperature As Integer
Dim Maxsteps As Integer
Dim Backlash As Integer
Dim Bldir As Byte                                           'Backlashrichtung 50=IN 51=out
Dim Dutycycle As Byte
Dim Stepdelay As Byte
Dim Stepsize As Byte
Dim Moveout As Bit
Dim Els_in_flag As Bit                                      'Auslösebit für inneren ELS
Dim Els_out_flag As Bit                                     'Auslösebit für äißeren ELS
Dim Els_in_avail As Bit                                     'Anwesenheitsbit für inneren ELS
Dim Els_out_avail As Bit                                    'Anwesenheitsbit für äußeren ELS
Dim Fastkey As Bit
Dim Keypressed As Bit
'Dim Isrs232 As Bit
Dim Astop As Bit
Dim Tmpb As Byte
Dim Bs As Byte
Dim I1 As Integer
Dim Steps As Integer
Dim Timerdelay As Integer
Dim Ar(2) As Byte                                           'Array für die Register
Dim Temperatur As Integer
Dim Halb As Byte
Dim Comm_en As Byte







'Variablen für Funktionen
Dim Ts As String * 10
Dim Laenge As Integer
Dim T As String * 5
Dim Valuestr As String * 8
Dim Cs As String * 1

Dim Bytesum As Integer
Dim Charval As Byte
Dim Zeichtmp As String * 1
Dim Blenable As Bit




' Konfigurationsvariablen im EEprom
Dim Emaxsteps As Eram Integer
Dim Ebacklash As Eram Integer
Dim Ebldir As Eram Byte
Dim Edutycycle As Eram Byte
Dim Estepdelay As Eram Byte
Dim Estepsize As Eram Byte


'Deklarationen für den Datentransfer per RS232
'Datenstruktur für den Commandstring
Dim Subcommandbyte As Byte                                  '1 byte
Dim V1 As Word                                              '2 byte
Dim V2 As Word                                              '2 byte
Dim V3 As Word                                              '2 byte
Dim Chksum As Byte                                          '1 byte

Dim Command(8) As Byte At Subcommandbyte Overlay            ' overlay für den Commandstring
Dim C As Byte                                               'Zeichen puffer
Dim D As Byte                                               'Zähler
Dim Commandstr As String * 8
Dim Subcommandchar As String * 1


'Subfunktionen
'Declare Sub Empfang
Declare Function Encodevalue(byval Value As Integer ) As String
Declare Function Valuetoint(cmd As String) As Integer
Declare Function Getcommandvalue As Integer
Declare Function Computechecksum(cmd As String) As String
Declare Sub Responsecommand(byval Subchar As String , Cmd As String)
Declare Sub Moveincommand(w As Byte)
Declare Sub Moveoutcommand(w As Byte)
Declare Sub Readtemperature





'*******************************************************************************
'Hauptprogrammstart  initialisierung
'*******************************************************************************
Disable Interrupts                                          'zuerst alle Interrupts Disablen
Disable Timer0
Disable Int0
Disable Int1
Stop Timer0

                                                            'Variablen vom EEProm holen
Maxsteps = Emaxsteps
Backlash = Ebacklash
Bldir = Ebldir
Dutycycle = Edutycycle
Stepdelay = Estepdelay
Stepsize = Estepsize

Reset Els_in_flag                                           'Endlagenschalter-Flags rücksetzen
Reset Els_out_flag

Reset Els_in_avail                                          'els verfügbarkeit ausschalten
Reset Els_out_avail
Reset Blenable
Portd.2 = 0
Portd.3 = 0


Reset Pind.2                                                'port auf Low ziehen
Reset Pind.3                                                'für ELS

Set Stepperreset                                            'Treiber Resetten
Reset Stepperreset
Set Stepperreset

Maxsteps = Abs(maxsteps)
Backlash = Abs(backlash)

Set Stepperenable                                           ' Steppertreiber enablen
Set Halfstep                                                'auf hablschritt schalten
Set Portc.2                                                 'Clock von L297 auf High legen

Enable Interrupts
Enable Timer0
Call Readtemperature
Comm_en = 1                                                 'Comm Einschalten



'ein stück nach außen fahren
'Wenn endlagenschalter da sind, dann bis zum inneren fahren
'Falls nur der innere angeschlossen ist ist das folgende nötig um ihn zu schließen
'sonst schlägt der nöchste Test fehl!

Tmpb = 1
Actualposition = 1
Value = 100                                                 'initsteps Schritte nach außen fahren
Call Moveoutcommand(tmpb)




'Jetzt sollte der innere Endlagenschalter geschlossen sein wenn er vorhanden ist
'-------------------------------------------------------------------------------
'Test auf Endlagenschalter -einer muss immer geschlossen sein! ( weil Öffner)


If Pind.3 = 0 Then                                          'innerer nicht da
  Disable Int1                                              'INT1 abschalten da nicht nötig
  Reset Els_in_avail                                        'FLag für Anwesenheit wegschalten
                                                          'Dann wieder zu ausgangsposition
  Call Moveincommand(tmpb)
Else
  Config Int1 = Falling                                     'Fallende Flanke an INT1 für Endlagenschalter Out
  On Int1 Els_in                                            'ISR für Endlagenschalter Äußere Position
  Enable Int1                                               'Wenn gefunden
  Set Els_in_avail
End If





'Wenn innere Endlagenschalter da ist, dann soweit nach innen fahren bis er auslöst
If Els_in_avail = 1 Then
   Value = Maxsteps + 500
   Call Moveincommand(tmpb)
End If


'jetzt test des äußeren ELS
If Pind.2 = 0 Then                                          'äußerer nicht da
  Disable Int0
  Reset Els_out_avail
Else
  Config Int0 = Falling                                     'fallende Flanke an INT0 für Endlagenschalter In
  On Int0 Els_out                                           'ISR für Endlagenschalter Innere position
  Enable Int0                                               'Interrupts einschalten
  Set Els_out_avail
End If


Actualposition = 1





'###############################################################################


'Hauptschleife
'Warter auf ein Zeichen von der RS232 Schnittstelle
'===============================================================================
'###############################################################################

Reset Keypressed

D = 0                                                       ' auf steuerzeichen warten
Do
 If Ischarwaiting() = 1 Then
  If Comm_en = 1 Then Gosub Empfang                         'Wenn einzeichen kommt dann Empfangsroutine aufrufen
 End If
                                                         'Wenn es ein kommando war
 If D = 9 Then Gosub Decodecommand
                                                          'Taste gedrückt?
  If Pind.4 = 0 Then                                        'Taste IN Gedrückt
    Reset Fastkey
    Gosub Moveinkey
  End If

  If Pind.5 = 0 Then                                        'Taste IN Gedrückt
    Set Fastkey
    Gosub Moveinkey
  End If

  If Pind.6 = 0 Then                                        'Taste IN Gedrückt
    Reset Fastkey
    Gosub Moveoutkey
  End If

  If Pind.7 = 0 Then                                        'Taste IN Gedrückt
    Set Fastkey
    Gosub Moveoutkey
  End If

  If Keypressed = 1 And Pind.4 = 1 And Pind.5 = 1 And Pind.6 = 1 And Pind.7 = 1 Then
   Reset Keypressed                                         ''Wenn taste losgelassen
   Temp1 = Encodevalue(actualposition)
   Responsecommand "D" , Temp1
   Gosub Dodutycicle                                        'DutyCicle setzen

  End If

Loop
End







'===============================================================================

'Bewegt den Motor um die in Value angegebenen Steps nach innen
'===============================================================================
Moveinkey:

   Set Keypressed
   Reset Dirout                                             'Richtung nach innen
   Reset Moveout

   Set Stepperenable

   If Actualposition < 2 Then
    Return
   End If

   If Fastkey = 1 Then Count = Stepdelay Else Count = Stepsize * Stepdelay

   Reset Portc.2
   Set Portc.2
   Decr Actualposition
   'Print "I"
   Reset Els_out_flag
   Reset Els_in_flag
   Waitms Count


Return

'===============================================================================

'Bewegt den Motor um die in Value angegebenen Steps nach innen
'===============================================================================
Moveoutkey:

   Set Keypressed
   Set Dirout                                               'Richtung nach innen
   Set Moveout

   Set Stepperenable

   If Actualposition = Maxsteps Then
    Return
   End If

   If Fastkey = 1 Then Count = Stepdelay Else Count = Stepsize * Stepdelay

   Reset Portc.2
   Set Portc.2
   'Print "O"
   Incr Actualposition
   Reset Els_out_flag
   Reset Els_in_flag
   Waitms Count

Return


'===============================================================================











' Falls ein "F" gesendet wird, werden die Nachfolgenden 8 Byte gelesen
'===============================================================================
Empfang:
Stop Timer0                                                 'Empfang hat vorrang vor
C = Inkey()                                                 'Motor
   If D > 0 Then
      If D < 8 Then Command(d) = C
      Incr D
   Else
      If C = "F" Then Incr D
   End If
Return




'===============================================================================

'Liest den Commandstring aus und zerlegt ihn in seine Bestandteile
'===============================================================================
Decodecommand:

     Temp = ""
     D = 0

     Subcommandchar = Chr(subcommandbyte)

     'value auslesen
     For I = 2 To 7
       Temp = Temp + Chr(command(i))
     Next I

     If Temp = "VVVVVV" Then
      Gosub Dostop
      Return
     End If

     Set Blenable
     Value = Valuetoint(temp)

     Tmpb = 0
     Select Case Subcommandchar                             'in entsprechende Unterroutinen verzweigen
       Case "V" : Temp = "003.14"
                   Call Responsecommand( "V" , Temp)        'Firmware zurück geben
       Case "G" : Gosub Gotocommand
       Case "I" : Call Moveincommand(tmpb)
       Case "O" : Call Moveoutcommand(tmpb)
       Case "S" : Gosub Positioncommand
       Case "L" : Gosub Maxtravelcommand
       Case "B" : Gosub Backlashcommand
       Case "C" : Gosub Configcommand
       Case "T" : Call Readtemperature
       Case Else : Print "Fehler"
     End Select
Return

'===============================================================================


'Bewegt von der aktuellen Position zur gewünschten Position
'===============================================================================
Gotocommand:

  Reset Astop
  Set Stepperenable
  Steps = 0
  If Value = 0 Then
    Temp1 = Encodevalue(actualposition)                     ' aktuelle position melden
    Call Responsecommand( "D" , Temp1)
  Else
     Steps = Value - Actualposition                         'Schrittdifferenz bestimmen
     If Steps < 0 Then                                      'Richtung bestimmen
       Reset Dirout
       Reset Moveout
       Gotochar = "I"
     Else
       Set Dirout
       Set Moveout
       Gotochar = "O"
     End If

     Steps = Abs(steps)

     'Backlashsteps je nach Bewegungsrichtung und BDir hinzufügen
     If Moveout = 0 Then
         Reset Els_in_flag
         If Bldir = 51 Then Steps = Steps + Backlash        'BLSteps weiter bewegen
     Else
         Reset Els_out_flag
         If Bldir = 50 Then Steps = Steps + Backlash        'BLSteps weiter bewegen
     End If



     If Steps > 0 Then Start Timer0                         'Motor an
  End If
Return







'===============================================================================
'Setzt die aktuelle Position. Wenn Value =0 dann wird die aktuelle Position zurückgegeben
'===============================================================================
Positioncommand:

   If Value = 0 Then                                        'Position lesen
      Temp = Encodevalue(actualposition )
      Call Responsecommand( "D" , Temp)
   Else                                                     'Position schreiben
      Actualposition = Value
      Temp = Encodevalue(actualposition )
      Call Responsecommand( "D" , Temp )
   End If
Return
'===============================================================================


'Setzt den aktuellen Maxtravel. Wenn Value =0 dann wird der aktuelle Wert zurückgegeben
'===============================================================================
Maxtravelcommand:
   If Value = 0 Then                                        'Wert lesen
      Temp = Encodevalue(maxsteps )
      Call Responsecommand( "L" , Temp)
   Else
      Maxsteps = Value                                      'Wert schreiben
      Temp = Encodevalue(maxsteps )
      Emaxsteps = Maxsteps                                  'ins EEprom
      Call Responsecommand( "L" , Temp )
   End If
Return
'===============================================================================

'===============================================================================
Backlashcommand:

   If Command(2) = 48 Then                                  'Wenn eine 0 abfrage
      If Bldir < 50 Then Bldir = 50                         'Defaultwert setzen
      If Bldir > 52 Then Bldir = 50
      Temp = Encodevalue(backlash)
      Temp = Chr(bldir) + Temp
      Call Responsecommand( "B" , Temp)
   Else
      Bldir = Command(2)                                    'Richtung auslesen
      If Bldir < 50 Then Bldir = 50                         'Defaultwert setzen
      If Bldir > 52 Then Bldir = 50
      Backlash = Value
      Gosub Configbacklash                                  'Config in EEprom

      Temp = Encodevalue(backlash )
      Ebacklash = Backlash                                  'ins EEprom
      Temp = Chr(bldir) + Temp                              'Backlashconfig updaten
      Call Responsecommand( "B" , Temp)
   End If
Return
'===============================================================================

'===============================================================================
Configcommand:
     Bs = 0
     For I = 2 To 4
       Bs = Bs + Command(i)
     Next I

     If Bs = 144 Then                                       '3x ASCII zeichen "0" aufsummiert
      Temp = Chr(dutycycle)                                 'Aktuelle Werte zurückgeben
      Temp = Temp + Chr(stepdelay)
      Temp = Temp + Chr(stepsize)
      Temp = Temp + "000"                                   'neue werte zurück geben
      Call Responsecommand( "C" , Temp)
     Else
      Dutycycle = Command(2)                                'Neue Werte schreiben
      Stepdelay = Command(3)
      Stepsize = Command(4)
      Edutycycle = Dutycycle
      Estepsize = Stepsize
      Estepdelay = Stepdelay                                'ins EEprom
      Gosub Dodutycicle
      Temp = Chr(dutycycle) + Chr(stepdelay) + Chr(stepsize) + "000"       'Aktuelle Werte zurückgeben
      Call Responsecommand( "C" , Temp)
   End If
Return
'===============================================================================

'===============================================================================
Configbacklash:
   Ebacklash = Backlash
   Ebldir = Bldir
Return

'===============================================================================

Dobacklash:

   Waitms 50
   If Backlash = 0 Then
     Return
   End If
                                                    'Backlash Korrektiur nach außen
   If Bldir = 51 Then
      If Moveout = 1 Then                                   'abbrechen wenn bewegung nach außen und BackLout
        Return
      End If

      Set Dirout                                            'Richtung nach Außen
      'Set Moveout
      Set Stepperenable
      Steps = Backlash
      For I1 = 1 To Steps
       Reset Portc.2
       Set Portc.2
       Incr Actualposition
       Waitms 10
      Next I1
   Else                                                     'Backlash Korrektur nach innen
      If Moveout = 0 Then                                   'Wenn nach innen gefahren wurde dann exit
        Return
      End If

      Reset Dirout                                          'Richtung nach innen
      'Reset Moveout
      Set Stepperenable
      Steps = Backlash
      For I1 = 1 To Steps
       Reset Portc.2
       Set Portc.2
       Decr Actualposition
       Waitms 10
      Next I1
   End If

Return

'===============================================================================

Dodutycicle:
 If Dutycycle < 50 Then
   Reset Stepperenable
 Else
   Set Stepperenable
 End If
Return


'==============================================================================
Dostop:                                                     'Motor anhalten und Position zurück geben
   Stop Timer0
   Set Astop

   If Blenable = 1 Then
     Gosub Dobacklash
   End If
                                                      'Backlashkompensation
   If Actualposition < 1 Then
    Actualposition = 1
   End If

   Temp1 = Encodevalue(actualposition)

   Call Responsecommand( "D" , Temp1)

   Gosub Dodutycicle                                        'DutyCicle setzen
   Comm_en = 1
Return






'Ab hier Funktionen und SUBs



'===============================================================================

' Berechnet den String mit vorangestellten Nullen einesnumerischen Wert
'===============================================================================
Function Encodevalue(byval Value As Integer ) As String

  Ts = Str(value)
  Laenge = Len(ts)

  For I = Laenge To 4
    Ts = "0" + Ts                                           'nullen voranstellen
  Next I

  Encodevalue = Ts


End Function



' Berechnet den numerischen Wert im Kommando und gibt ihn zurück
'===============================================================================
Function Valuetoint(cmd As String) As Integer

  T = Right(cmd , 5)
  Valuetoint = Val(t)

End Function
'===============================================================================





' Berechnet das Checksummenzeichen für den Commandstring
'===============================================================================
Function Computechecksum(byval Cmd As String) As String * 1

  Bytesum = 0
  For I = 1 To Len(cmd)
    Zeichtmp = Mid(cmd , I , 1)
    Charval = Asc(zeichtmp)
    Bytesum = Bytesum + Charval
  Next I

  Bytesum = Bytesum Mod 256
  Computechecksum = Chr(bytesum)

End Function
'===============================================================================


' hängt die Prüfsumme an den Commandstring und gibt ihn auf die RS232 Schnittst.
'===============================================================================

Sub Responsecommand(byval Subchar As String , Cmd As String)

  Dim Resp As String * 10

  If Len(cmd) = 5 Then Cmd = "0" + Cmd

   Resp = "F"                                               '+ Subchar + Cmd + Computechecksum(resp)
   Resp = Resp + Subchar
   Resp = Resp + Cmd
   Schecksum = Computechecksum(resp)
   Resp = Resp + Schecksum
  Print Resp

End Sub



'===============================================================================

'Bewegt den Motor um die in Value angegebenen Steps nach innen
'===============================================================================
Sub Moveincommand(w As Byte)
   Gotochar = ""                                            ' Kein goto, also keine Meldung bei jedem Step

   Reset Astop
   Reset Els_out_flag
   Reset Dirout                                             'Richtung nach innen
   Reset Moveout
   Set Stepperenable                                        'Stepper einschalten
                                                             'Wenn Backlash nach außen
   If Bldir = 51 Then
     Steps = Abs(value) + Backlash
   Else
     Steps = Abs(value)                                     ' ansonsten nichts hinzufügen
   End If

   If Steps > 0 Then
     Start Timer0
   End If                                                   'Motor an

   If W = 1 Then
    Do
    Loop Until Astop = 1
   End If

End Sub
'===============================================================================





'Bewegt den Motor um die in Value angegebenen Steps nach außen
'===============================================================================
Sub Moveoutcommand(w As Byte)

   Gotochar = ""                                            ' Kein goto, also keine Meldung bei jedem Step

   Reset Astop
   Reset Els_in_flag
   Set Dirout                                               'Richtung nach Außen
   Set Moveout
   Set Stepperenable                                        'Stepper Einschalten

   If Bldir = 50 Then
     Steps = Abs(value) + Backlash
   Else
     Steps = Abs(value)                                     'ansonsten nichts hinzufügen
   End If

   If Steps > 0 Then
     Start Timer0                                           'Motor an
   End If

   If W = 1 Then
     Do
     Loop Until Astop = 1
   End If


End Sub







'===============================================================================



'===============================================================================

' liest die Temperatur aus dem DS1820 und codiert sie
'===============================================================================
Sub Readtemperature

   Waitms 300
   1wreset                                                  'reset the device
   1wwrite &HCC                                             'read ROM command
   1wwrite &H44
   Waitms 300                                               'read ROM command
   1wreset                                                  'reset the device
   1wwrite &HCC                                             'read ROM command
   1wwrite &HBE

   For I = 1 To 2
    Ar(i) = 1wread()                                        'place into array
   Next


   Ds1820 = Ar(1)
   Halb = Ar(1) And 1

   If Ar(2) > 0 Then Ds1820 = 0 Else Shift Ds1820 , Right


   Ds1820 = Ds1820 + 273
   Ds1820 = Ds1820 * 2
   Ds1820 = Ds1820 + Halb


   Temp = Encodevalue(ds1820 )

   Call Responsecommand( "T" , Temp )                       'Wert zurückgeben

End Sub

'===============================================================================









'Sub Reversebl
'Dim I2 As Integer

'   If Backlash = 0 Then Exit Sub

'  If Bldir = 51 Then
'      If Moveout = 0 Then Exit Sub
'      Reset Dirout                                          'Richtung nach innen
'      Reset Moveout
'      Set Stepperenable
'      Steps = Backlash
'      For I2 = 1 To Steps
'       Reset Portc.2
'       Set Portc.2
'       Decr Actualposition
'       Waitms 5
'      Next I2
'   Else
'      If Moveout = 1 Then Exit Sub                          'abbrechen wenn bewegung nach außen und BackLout
'      Set Dirout                                            'Richtung nach Außen
'      Set Moveout
'      Set Stepperenable
'      Steps = Backlash
'      For I2 = 1 To Steps
'       Reset Portc.2
'       Set Portc.2
'       Incr Actualposition
'       Waitms 5
'      Next I2
'   End If

'End Sub


'===============================================================================
 'INTERRUPT HANDLER ROUTINEN
'===============================================================================
Tim0_isr:
'Timer initialisieren
'TImer löst bei 6MHz alle mit Prescale 1024 etwa alle 0,17066 msek aus
' daher wird mit 5.85 x 0,17066 etwa 1ms erreicht (~6)
' um auf 3ms zu kommen wird der Wert mit 3 Multipliziert =18
'Diese wird mit dem Wert in Stepdelay multipliziert  und vom Überlauf abgezogen

  Timerdelay = 6 * Stepdelay
  Timer0 = 255 - Timerdelay                                 'Timerwert in ms setzen
  Comm_en = 0

  Set Stepperenable
  Reset Portc.2
  Set Portc.2
                                                             'Puls gegen Low und damit takten'
  Decr Steps                                                'relative Schrittzahl verringern

  If Moveout = 0 Then
    Decr Actualposition
  Else
    Incr Actualposition
  End If

  If Steps = 0 Then
    Gosub Dostop
  End If                                                    'Wenn anzahl noch zu tätigender schritte=0 dann Timer abschalten

  If Els_in_flag = 1 Then
    Gosub Dostop
  End If                                                    'Ebenso wenn ELS in Auslöst

  If Els_out_flag = 1 Then
    Gosub Dostop
  End If                                                    'Ebenso wenn ELS Out auslöst

Return




'Interrupt Handler für Int0 Interrupt = Endlagenschalter In
Els_in:
      Set Els_in_flag
      Actualposition = 1
Return


'Interrupt Handler für Int1 Interrupt = Endlagenschalter Out
Els_out:
     Set Els_out_flag
Return