1 | 'Uart Protokoll:
|
2 | 'Drive : R_l -pwm_l -r_r -pwm_r
|
3 | 'drive:1-120-1-130
|
4 |
|
5 |
|
6 | $regfile = "m16def.dat"
|
7 | $crystal = 16000000
|
8 | $baud = 9600
|
9 |
|
10 | $include "sub.bas"
|
11 |
|
12 |
|
13 | Config Serialin = Buffered , Size = 30 , Bytematch = 13
|
14 | Declare Sub Serial0charmatch()
|
15 |
|
16 |
|
17 |
|
18 | 'Config Outputs
|
19 |
|
20 |
|
21 | Config Portc.4 = Output 'M_left1a
|
22 | M_left1a Alias Portc.4
|
23 | Config Portc.5 = Output 'M_left2a
|
24 | M_left2a Alias Portc.5
|
25 | Config Portc.6 = Output 'M_left3a
|
26 | M_right3a Alias Portc.6
|
27 | Config Portc.7 = Output 'M_left4a
|
28 | M_right4a Alias Portc.7
|
29 |
|
30 |
|
31 | 'Declare Motor-PWM
|
32 | Config Portb.0 = Output 'm-left-enable
|
33 | M_left_e Alias Portb.0
|
34 | Config Portb.1 = Output 'm-right-enable
|
35 | M_right_e Alias Portb.1
|
36 |
|
37 |
|
38 | 'Config Inputs
|
39 | Config Portd.2 = Input 'emergency-out
|
40 | Em_out Alias Portc.0
|
41 | Em_out = 1 'enable pullup
|
42 |
|
43 |
|
44 | 'Config Interrupts
|
45 | ' Config Int0 = Falling 'fallende Flanke
|
46 | ' On Int0 On_em_out
|
47 | ' Enable Int0
|
48 |
|
49 |
|
50 | Enable Interrupts
|
51 |
|
52 |
|
53 | 'Config Var's
|
54 | Dim Tmp As Byte
|
55 | Dim New_command As String * 30
|
56 | Dim Incoming_array(5) As String * 30
|
57 | Dim Command_array(5) As String * 10
|
58 |
|
59 |
|
60 | Dim Pwm_1 As Integer
|
61 | Dim Pwm_2 As Integer
|
62 |
|
63 |
|
64 | 'Dim Direc_l As String * 1
|
65 | 'Dim Pwm_l As String * 3
|
66 | 'Dim Direc_r As String * 1
|
67 | 'Dim Pwm_r As String * 3
|
68 |
|
69 |
|
70 | Dim Drive_command(4) As String * 15
|
71 |
|
72 |
|
73 |
|
74 | Declare Sub Exc_drive(byval Direc_l As String , Byval Pwm_l As String , Direc_r As String , Byval Pwm_r As String)
|
75 |
|
76 |
|
77 |
|
78 | Do
|
79 |
|
80 |
|
81 |
|
82 | If New_command <> "" Then
|
83 |
|
84 | New_command = Ucase(new_command)
|
85 |
|
86 | 'Anweisung aufteilen
|
87 | Tmp = Split(new_command , Incoming_array(1) , ":")
|
88 |
|
89 | Select Case Incoming_array(1)
|
90 | Case "DRIVE"
|
91 | Tmp = Split(incoming_array(2) , Command_array(1) , "-")
|
92 | ' Drive_command(1) = Command_array(1)
|
93 | ' Drive_command(2) = Command_array(2)
|
94 | ' Drive_command(3) = Command_array(3)
|
95 | ' Drive_command(4) = Command_array(4)
|
96 |
|
97 |
|
98 | 'Direc_l = Command_array(1)
|
99 | ' Pwm_l = Command_array(2)
|
100 | ' Direc_r = Command_array(3)
|
101 | ' Pwm_r = Command_array(4)
|
102 |
|
103 | 'Print Command_array(1) ; " " ; Command_array(2) ; " " ; Command_array(3) ; " " ; Command_array(4)
|
104 |
|
105 | ' Gosub Exc_drive
|
106 | Print Command_array(1) ; " "
|
107 | Print Command_array(2) ; " "
|
108 | Print Command_array(3) ; " "
|
109 | Print Command_array(4) ; " "
|
110 |
|
111 | Call Exc_drive(command_array(1) , Command_array(2) , Command_array(3) , Command_array(4))
|
112 |
|
113 |
|
114 | Case Else
|
115 | Print "ERROR: unknown command"
|
116 | End Select
|
117 |
|
118 | New_command = ""
|
119 | End If
|
120 |
|
121 |
|
122 | 'Wait 100
|
123 |
|
124 |
|
125 | 'Call Exc_drive( "1" , "120" , "1" , "130")
|
126 |
|
127 |
|
128 | Loop
|
129 | End
|
130 |
|
131 |
|
132 |
|
133 | Sub Serial0charmatch()
|
134 |
|
135 | Input New_command Noecho
|
136 | Print New_command
|
137 |
|
138 | End Sub
|
139 |
|
140 |
|
141 |
|
142 |
|
143 | 'Exc_drive:
|
144 | ' If Drive_command(1) = "1" Then Gosub Forward_left 'drive forward
|
145 | ' If Drive_command(1) = "0" Then Gosub Backward_left 'drive backward
|
146 |
|
147 | ' If Drive_command(3) = "1" Then Gosub Forward_right 'drive forward
|
148 | ' If Drive_command(3) = "0" Then Gosub Backward_right 'drive backward
|
149 |
|
150 | ' Pwm_1 = Val(drive_command(2))
|
151 | ' Pwm_2 = Val(drive_command(4))
|
152 |
|
153 | ' Pwm1a = Pwm_1
|
154 | ' Pwm1b = Pwm_2
|
155 |
|
156 | ' Print Pwm_1 ; " " ; Pwm_2
|
157 |
|
158 | 'Return
|
159 |
|
160 |
|
161 |
|
162 |
|
163 |
|
164 | Sub Exc_drive(byval Direc_l As String * 1 , Byval Pwm_l As String * 3 , Direc_r As String * 1 , Byval Pwm_r As String * 3)
|
165 |
|
166 | Print Direc_l ; " "
|
167 | Print Pwm_l ; " "
|
168 | Print Direc_r ; " "
|
169 | Print Pwm_r ; " "
|
170 |
|
171 |
|
172 | If Direc_l = "1" Then Gosub Forward_left 'drive forward
|
173 | If Direc_l = "0" Then Gosub Backward_left 'drive backward
|
174 |
|
175 | If Direc_r = "1" Then Gosub Forward_right 'drive forward
|
176 | If Direc_r = "0" Then Gosub Backward_right 'drive backward
|
177 |
|
178 | Pwm_1 = Val(pwm_l)
|
179 | Pwm_2 = Val(pwm_r)
|
180 |
|
181 | Pwm1a = Pwm_1
|
182 | Pwm1b = Pwm_2
|
183 |
|
184 | 'Print Pwm_1 ; " " ; Pwm__l ; " " ; Direc__r ; " " ; Pwm__r
|
185 |
|
186 |
|
187 | End Sub
|
188 |
|
189 |
|
190 |
|
191 |
|
192 | Forward_left:
|
193 | M_left1a = 1
|
194 | M_left2a = 0
|
195 | Return
|
196 |
|
197 |
|
198 | Backward_left:
|
199 | M_left1a = 0
|
200 | M_left2a = 1
|
201 | Return
|
202 |
|
203 |
|
204 | Forward_right:
|
205 | M_right4a = 1
|
206 | M_right3a = 0
|
207 | Return
|
208 |
|
209 |
|
210 | Backward_right:
|
211 | M_right4a = 0
|
212 | M_right3a = 1
|
213 | Return
|
214 |
|
215 |
|
216 | Stop_drive:
|
217 |
|
218 | Pwm1a = 0
|
219 | Pwm1b = 0
|
220 |
|
221 | M_left1a = 0
|
222 | M_left2a = 0
|
223 |
|
224 | M_right4a = 0
|
225 | M_right3a = 0
|
226 |
|
227 | Return
|