-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathengines.fs
178 lines (151 loc) · 5.1 KB
/
engines.fs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
include piceeprom.fs
variable mul-acc-high variable mul-acc-low
variable mul-fac 1
variable mul-res-high variable mul-res-low
\ Multiply a 16 bits value in mul-acc by a 8 bits value in mul-fac -
\ the result must fit in 16 bits and is stored in mul-res. This is a
\ destructive operation for both operands.
: mult ( -- )
0 mul-res-high ! 0 mul-res-low !
begin
\ While multiplicand is not 0
mul-fac @ 0= if exit then
\ Shift multiplicand right and act if low bit was set
c bit-clr mul-fac rrf!
c bit-set? if
\ Add low byte of accumulator to result
mul-acc-low @ mul-res-low +! c bit-set? if 1 mul-res-high +! then
\ Add high byte of accumulator to result
mul-acc-high @ mul-res-high +!
then
\ Shift accumulator left
c bit-clr mul-acc-low rlf! mul-acc-high rlf!
again
;
variable position1 variable position2 variable position3 variable position4
variable ticks1 variable ticks2
\ Reset ticks counter
: reset-ticks ( -- ) 0 ticks1 ! 0 ticks2 ! ;
\ Add and remove one tick
: inc-ticks ( -- ) 1 ticks2 +! z bit-set? if 1 ticks1 +! then ;
: dec-ticks ( -- ) 1 >w ticks2 w-! c bit-set? if 1 ticks1 -! then ;
\ Add 16 bits ticks to current 32bits position. This also resets tick
\ counter.
: update-position ( -- )
ticks2 @ position4 +! c bit-set? if 1 ticks1 +! then
ticks1 @ position3 +! c bit-set? if
1 position2 +! c bit-set? if
1 position1 +!
then
then
reset-ticks
;
\ Add acceleration (32 bits) to velocity (32 bits). Also, limit velocity
\ to the limit (32 bits) value. Returns a non-null value if velocity has
\ been limited.
variable accel1 variable accel2 variable accel3 variable accel4
variable vel1 variable vel2 variable vel3 variable vel4
variable limit1 variable limit2 variable limit3 variable limit4
: copy-limit-4 ( -- true ) limit4 @ vel4 ! 1 ;
: copy-limit-3 ( -- true ) limit3 @ vel3 ! copy-limit-4 ;
: copy-limit-2 ( -- true ) limit2 @ vel2 ! copy-limit-3 ;
: copy-limit-1 ( -- true ) limit1 @ vel1 ! copy-limit-2 ;
: limit-velocity-+ ( -- limited? )
vel1 @ limit1 @ < if 0 exit then
limit1 @ vel1 @ < if copy-limit-1 exit then
vel2 @ limit2 @ < if 0 exit then
limit2 @ vel2 @ < if copy-limit-2 exit then
vel3 @ limit3 @ < if 0 exit then
limit3 @ vel3 @ < if copy-limit-3 exit then
vel4 @ limit4 @ < if 0 exit then
limit4 @ vel4 @ < if copy-limit-4 exit then
1
;
: update-velocity-+ ( -- limited? )
vel4 @ accel4 +! z bit-set? if
1 vel3 +! z bit-set? if
1 vel2 +! z bit-set? if
1 vel1 +!
then
then
then
vel3 @ accel3 +! z bit-set? if
1 vel2 +! z bit-set? if
1 vel1 +!
then
then
vel2 @ accel2 +! z bit-set? if
1 vel1 +!
then
limit-velocity-+
;
: limit-velocity-- ( -- limited? )
limit1 @ vel1 @ < if 0 exit then
vel1 @ limit1 @ < if copy-limit-1 exit then
limit2 @ vel2 @ < if 0 exit then
vel2 @ limit2 @ < if copy-limit-2 exit then
limit3 @ vel3 @ < if 0 exit then
vel3 @ limit3 @ < if copy-limit-3 exit then
limit4 @ vel4 @ < if 0 exit then
vel4 @ limit4 @ < if copy-limit-4 exit then
1
;
: update-velocity-- ( -- limited ?)
vel4 @ accel4 -! c bit-set? if
1 >w vel3 w-! c bit-set? if
vel2 w-! c bit-set? if
1 vel1 -!
then
then
then
vel3 @ accel3 -! c bit-set? if
1 >w vel2 w-! c bit-set? if
1 vel1 -!
then
then
vel2 @ accel2 -! c bit-set? if
1 vel1 -!
then
limit-velocity--
;
\ Coding wheels transitions. The edges are memorized, sensors are called A and B.
\ A low edge is 00, A high edge is 01, B low edge is 10, B high edge is 11.
\ In the following array, 1 means forward, -1 backward, and 0 reverse. We put a
\ 2 to indicate an impossible value (two successive identical transitions), which can
\ happen if the robot stops right at a transition.
\ The index is a four bits value: 2 (high) bits with the previous transition and
\ 2 (low) bits with the latest transition.
\
\ 01 _____ 00 01 _____ 00
\ _____| |_____| |______ A sensor
\ ___ 10 11 _____ 10 11 _____ 10
\ |_____| |_____| |______ B sensor
eetable transitions
table> ( 00 00 ) 2 ( 00 01 ) 0 ( 00 10 ) 1 ( 00 11 ) -1
table> ( 01 00 ) 0 ( 01 01 ) 2 ( 01 10 ) -1 ( 01 11 ) 1
table> ( 10 00 ) -1 ( 10 01 ) 1 ( 10 10 ) 2 ( 10 11 ) 0
table> ( 11 00 ) 1 ( 11 01 ) -1 ( 11 10 ) 0 ( 11 11 ) 2
end-table
variable previous-transitions
\ Given a transition, return the current direction: 1 (forward), -1 (backward),
\ 0 (reverse) or 2 (impossible: probably a glitch, should be ignored).
: direction ( transition -- )
previous-transitions @ 2* 2* + $f and dup previous-transitions ! transitions ;
variable current-direction
: handle-tick ( -- )
current-direction @ 0< if dec-ticks exit then inc-ticks ;
: handle-direction ( direction -- )
dup 0= if \ Reverse direction
drop current-direction @ negate current-direction ! handle-tick exit
then
dup 2 = if \ Invalid move
drop exit
then
current-direction ! handle-tick
;
: handle-event ( transition -- ) direction handle-direction ;
: init ( -- )
reset-ticks
1 current-direction !
0 previous-transitions !
;