' 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.0 = Output
Config Portb.1 = Output
Led_rt Alias Portb.0
Led_gr Alias Portb.1
'
' motor
Config Portc.5 = Output
Mot_en Alias Portc.5
Config Portc.4 = Output
Config Portc.3 = Output
Mot1_stp Alias Portc.4
Mot1_dir Alias Portc.3
Config Portc.2 = Output
Config Portc.1 = Output
Mot2_stp Alias Portc.2
Mot2_dir Alias Portc.1
Config Portd.6 = Output
Config Portd.7 = 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(1 , 0)
Call Set_motor_speed(2 , 0)
Call Set_motor_speed(3 , 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(i + 4)
Hex3b(i) = Msg_buf(i + 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(1 , V1)
Call Set_motor_speed(2 , V2)
Call Set_motor_speed(3 , 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