' kleines bespielprogramm zur ansteuerung
' von drei motoren mit omni wheels
' malte t. ahlers 2013
' infos auf http://www.malteahlers.de
'
' compiler
$regfile = "m168def.dat"
$crystal = 8000000
$framesize = 64
$swstack = 64
$hwstack = 64
$baud = 9600
'
' leds
Config Portb.= Output
Config Portb.= Output
Led_rt Alias Portb.0
Led_gr Alias Portb.1
'
' motor
Config Portc.= Output
Mot_en Alias Portc.5
Config Portc.= Output
Config Portc.= Output
Mot1_stp Alias Portc.4
Mot1_dir Alias Portc.3
Config Portc.= Output
Config Portc.= Output
Mot2_stp Alias Portc.2
Mot2_dir Alias Portc.1
Config Portd.= Output
Config Portd.= Output
Mot3_stp Alias Portd.6
Mot3_dir Alias Portd.7
'
Config Timer0 = Timer , Prescale = 8
'
Declare Sub Set_motor_speed(byval Mot_nr As Byte , Byval V As Integer)
'
' uart
Const Uart_msg_len = 12
Const Uart_buf_len = Uart_msg_len + 2
Const Uart_msg_dim = Uart_msg_len + 1
Dim Uart_buf(uart_buf_len) As Byte
Dim Uart_buf_i As Byte
Dim Msg_buf(uart_msg_dim) As Byte
Dim Msg_buf_i As Byte
Dim Msg As String * Uart_msg_len At Msg_buf Overlay
Dim Ub1 As Byte
Dim New_uart_msg As Byte
Uart_buf_i = 1
Msg_buf_i = 1
'
Dim Hex1b(5) As Byte
Dim Hex1 As String * 4 At Hex1b Overlay
Dim Hex2b(5) As Byte
Dim Hex2 As String * 4 At Hex2b Overlay
Dim Hex3b(5) As Byte
Dim Hex3 As String * 4 At Hex3b Overlay
'
Dim I As Byte
'
Dim Vx As Integer
Dim Vy As Integer
Dim V1 As Integer
Dim V2 As Integer
Dim V3 As Integer
Dim V_rot As Integer
'
Dim Mot1_cnt As Word
Dim Mot1_cnt_th As Word
Dim Mot2_cnt As Word
Dim Mot2_cnt_th As Word
Dim Mot3_cnt As Word
Dim Mot3_cnt_th As Word
'
Dim Tmp_sng_1 As Single
Dim Tmp_sng_2 As Single
Dim Tmp_int_1 As Integer
'
Const T0_dt = 1.08e-4
Const Sin_p120 = 0.8660
Const Cos_p120 = -0.5
Const Sin_m120 = -0.8660
Const Cos_m120 = -0.5
'
On Timer0 Tmr0 , Nosave
On Urxc Rxc_isr
Enable Urxc
Enable Timer0
Enable Interrupts
'
Reset Mot_en
Call Set_motor_speed(, 0)
Call Set_motor_speed(, 0)
Call Set_motor_speed(, 0)
'
Do
   If New_uart_msg = 1 Then
      New_uart_msg = 0
      Toggle Led_rt
      '
      For I = 1 To 4
         Hex1b(i) = Msg_buf(i)
         Hex2b(i) = Msg_buf(+ 4)
         Hex3b(i) = Msg_buf(+ 8)
      Next
      '
      Vx = Hexval(hex1)
      Vy = Hexval(hex2)
      V_rot = Hexval(hex3)
      '
      V1 = Vx + V_rot
      '
      Tmp_sng_1 = Cos_p120 * Vx
      Tmp_sng_2 = Sin_p120 * Vy
      Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2
      Tmp_sng_1 = Tmp_sng_1 + V_rot
      V2 = Round(tmp_sng_1)
      '
      Tmp_sng_1 = Cos_m120 * Vx
      Tmp_sng_2 = Sin_m120 * Vy
      Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2
      Tmp_sng_1 = Tmp_sng_1 + V_rot
      V3 = Round(tmp_sng_1)
      '
      Call Set_motor_speed(, V1)
      Call Set_motor_speed(, V2)
      Call Set_motor_speed(, V3)
      '
   End If
Loop
'
End
'
Sub Set_motor_speed(byval Mot_nr As Byte , Byval V As Integer)
   '
   Select Case Mot_nr
      Case 1:
         If V < 0 Then
            Set Mot1_dir
            V = -v
         Else
            Reset Mot1_dir
         End If
         If V = 0 Then
            Tmp_int_1 = 255
         Else
            Tmp_sng_1 = V * T0_dt
            Tmp_sng_1 = 1 / Tmp_sng_1
            Tmp_sng_1 = Tmp_sng_1 - 1
            Tmp_int_1 = Round(tmp_sng_1)
         End If
         If Tmp_int_1 < 5 Then Tmp_int_1 = 5
         If Tmp_int_1 > 255 Then Tmp_int_1 = 255
         Mot1_cnt_th = Tmp_int_1
      Case 2:
         If V < 0 Then
            Set Mot2_dir
            V = -v
         Else
            Reset Mot2_dir
         End If
         If V = 0 Then
            Tmp_int_1 = 255
         Else
            Tmp_sng_1 = V * T0_dt
            Tmp_sng_1 = 1 / Tmp_sng_1
            Tmp_sng_1 = Tmp_sng_1 - 1
            Tmp_int_1 = Round(tmp_sng_1)
         End If
         If Tmp_int_1 < 5 Then Tmp_int_1 = 5
         If Tmp_int_1 > 255 Then Tmp_int_1 = 255
         Mot2_cnt_th = Tmp_int_1
      Case 3:
         If V < 0 Then
            Set Mot3_dir
            V = -v
         Else
            Reset Mot3_dir
         End If
         If V = 0 Then
            Tmp_int_1 = 255
         Else
            Tmp_sng_1 = V * T0_dt
            Tmp_sng_1 = 1 / Tmp_sng_1
            Tmp_sng_1 = Tmp_sng_1 - 1
            Tmp_int_1 = Round(tmp_sng_1)
         End If
         If Tmp_int_1 < 5 Then Tmp_int_1 = 5
         If Tmp_int_1 > 255 Then Tmp_int_1 = 255
         Mot3_cnt_th = Tmp_int_1
   End Select
End Sub
'
' *** ISRs ***
Rxc_isr:
   ' uart ringpuffer
   Uart_buf(uart_buf_i) = Inkey()
   '
   If Uart_buf(uart_buf_i) = 10 Then
      ' LF gefunden
      If Uart_buf_i = 1 Then
         Ub1 = Uart_buf_len
      Else
         Ub1 = Uart_buf_i - 1
      End If
      If Uart_buf(ub1) = 13 Then
         ' CR vor LF gefunden -> neue nachricht
         Ub1 = Ub1 + 2
         If Ub1 > Uart_buf_len Then
            Ub1 = Ub1 - Uart_buf_len
         End If
         Msg_buf_i = 1
         Do
            Msg_buf(msg_buf_i) = Uart_buf(ub1)
            If Ub1 < Uart_buf_len Then
               Incr Ub1
            Else
               Ub1 = 1
            End If
            If Msg_buf_i = Uart_msg_len Then
               New_uart_msg = 1
               For Ub1 = 1 To Uart_buf_len
                  Uart_buf(ub1) = 0
               Next
               Exit Do
            Else
               Incr Msg_buf_i
            End If
         Loop
      End If
   End If
   If Uart_buf_i < Uart_buf_len Then
      Incr Uart_buf_i
   Else
      Uart_buf_i = 1
   End If
Return
'
Tmr0:
   $asm
      PUSH R26
      PUSH R27
      IN R27, SREG
      PUSH R27
      ' timer0 preload
      ldi R26, 155
      Out Tcnt0 , R26
      ' alle stp auf 0
      CBI PORTc, PC4
      CBI PORTc, PC2
      CBI PORTD, PD6
   '-- V1
      lds R26, {Mot1_cnt_th}
      lds R27, {Mot1_cnt}
      '
      cpi R26, 255
      BREQ v1_incr
      '
      CP R27, R26
      BRLO V1_incr
      ' wenn kleiner verzweigen
      ' wenn größer: stp setzen und zähler auf 0
      SBI PORTc, PC4
      CLR R27
      RJMP V1_setcnt
   V1_incr:
      ' zähler inkrementieren
      inc R27
   V1_setcnt:
      STS {Mot1_cnt}, R27
   '-- V2
      lds R26, {Mot2_cnt_th}
      lds R27, {Mot2_cnt}
      '
      cpi R26, 255
      BREQ v2_incr
      '
      CP R27, R26
      BRLO V2_incr
      ' wenn kleiner verzweigen
      ' wenn größer: stp setzen und zähler auf 0
      SBI PORTc, PC2
      CLR R27
      RJMP V2_setcnt
   V2_incr:
      ' zähler inkrementieren
      inc R27
   V2_setcnt:
      STS {Mot2_cnt}, R27
   '-- V3
      lds R26, {Mot3_cnt_th}
      lds R27, {Mot3_cnt}
      '
      cpi R26, 255
      BREQ v3_incr
      '
      CP R27, R26
      BRLO V3_incr
      ' wenn kleiner verzweigen
      ' wenn größer: stp setzen und zähler auf 0
      SBI PORTD, PD6
      CLR R27
      RJMP V3_setcnt
   V3_incr:
      ' zähler inkrementieren
      inc R27
   V3_setcnt:
      STS {Mot3_cnt}, R27
      '
      POP R27
      !OUT SREG,R27
      POP R27
      POP R26
   $end Asm
Return