wf.fth (4299B)
1 4 CONSTANT CELLS 2 3 400 CONSTANT MAXSPEED 4 5 VARIABLE LPWM_NULL 6 VARIABLE LPWM_REVSCALE 7 VARIABLE LPWM_FWDSCALE 8 9 VARIABLE RPWM_NULL 10 VARIABLE RPWM_REVSCALE 11 VARIABLE RPWM_FWDSCALE 12 13 \ motor tuning constants 14 \ 388 LPWM_NULL ! 15 420 LPWM_NULL ! 16 450 LPWM_REVSCALE ! 17 564 LPWM_FWDSCALE ! 18 19 400 RPWM_NULL ! 20 \ 430 RPWM_NULL ! 21 \ 358 RPWM_REVSCALE ! 22 458 RPWM_REVSCALE ! 23 \ 489 RPWM_FWDSCALE ! 24 564 RPWM_FWDSCALE ! 25 26 : SCALESPEED ( speed pwmscale -- pwm ) 27 SWAP MAXSPEED MIN 28 SWAP MAXSPEED */ 29 ; 30 31 : SETLSPEED ( speed -- ) 32 DUP 0 >= 33 IF 34 NEGATE 35 LPWM_FWDSCALE @ SCALESPEED 36 LPWM_NULL @ + 37 ELSE 38 LPWM_REVSCALE @ SCALESPEED 39 LPWM_NULL @ SWAP - 40 THEN 41 PWML 42 ; 43 44 : SETRSPEED ( speed -- ) 45 DUP 0 >= 46 IF 47 RPWM_FWDSCALE @ SCALESPEED 48 RPWM_NULL @ + 49 ELSE 50 NEGATE 51 RPWM_REVSCALE @ SCALESPEED 52 RPWM_NULL @ SWAP - 53 THEN 54 PWMR 55 ; 56 57 : SETSPEED ( speed -- ) 58 DUP SETLSPEED SETRSPEED ; 59 60 : ACCEL ( n --- ) 61 0 SWAP DO I SETSPEED LOOP ; 62 63 : DECEL ( n --- ) 64 0 DO I SETSPEED -1 +LOOP ; 65 66 : STOPMOTORS ( --- ) 67 0 SETSPEED ; 68 69 \ LF/RRLEDS 70 \ RF/LRLEDS 71 \ LHSSENSE 72 \ RHSSENSE 73 \ LHSCHAN 74 \ RHSCHAN 75 76 \ ADC channels 77 $100 CONSTANT LHSCHAN 78 $8 CONSTANT RHSCHAN 79 80 : LDIST ( --- n ) 81 LHSCHAN ATOD 82 LF/RRLEDS SETPIN 83 LHSCHAN ATOD 84 LF/RRLEDS CLRPIN 85 SWAP - ; 86 87 : RDIST ( --- n ) 88 RHSCHAN ATOD 89 RF/LRLEDS SETPIN 90 RHSCHAN ATOD 91 RF/LRLEDS CLRPIN 92 SWAP - ; 93 94 : TESTLDIST ( --- n ) 95 CR BEGIN LDIST 5 MS 6 .r 13 EMIT KEY? UNTIL ; 96 97 : TESTRDIST ( --- n ) 98 CR BEGIN RDIST 5 MS 6 .r 13 EMIT KEY? UNTIL ; 99 100 \ ============================================================================ 101 102 STOPMOTORS 103 104 VARIABLE (FWALL_FAR) 105 : FWALL_FAR ( --- n ) 106 (FWALL_FAR) @ ; 107 108 VARIABLE (FWALL_NEAR) 109 : FWALL_NEAR ( --- n ) 110 (FWALL_NEAR) @ ; 111 112 VARIABLE (RTURN_SPEED) 113 : RTURN_SPEED ( --- n ) 114 (RTURN_SPEED) @ ; 115 116 VARIABLE (RTURN_RATE) 117 : RTURN_RATE ( --- n ) 118 (RTURN_RATE) @ ; 119 120 VARIABLE (LWALL_TARGET) 121 : LWALL_TARGET ( --- n ) 122 (LWALL_TARGET) @ ; 123 124 VARIABLE (FAST_SPEED) 125 : FAST_SPEED ( --- n ) 126 (FAST_SPEED) @ ; 127 128 VARIABLE (SLOW_SPEED) 129 : SLOW_SPEED ( --- n ) 130 (SLOW_SPEED) @ ; 131 132 VARIABLE (KP) 133 : KP ( --- n ) 134 (KP) @ ; 135 136 VARIABLE (KD) 137 : KD ( --- n ) 138 (KD) @ ; 139 140 VARIABLE PREV_RDIST 141 142 \ LHS - Front wall sensor 143 \ RHS - Left wall sensor 144 145 : SYNCH ( --- ) 146 5 MS ; 147 148 : TESTLHS ( --- ) 149 BEGIN 150 LDIST DUP 151 FWALL_FAR > IF LED1 SETPIN THEN 152 FWALL_NEAR > IF LED2 SETPIN THEN 153 SYNCH 154 LED1 CLRPIN 155 LED2 CLRPIN 156 KEY? 157 UNTIL 158 ; 159 160 : TESTRHS ( --- ) 161 BEGIN 162 RDIST LWALL_TARGET > IF LED1 SETPIN THEN 163 SYNCH 164 LED1 CLRPIN 165 KEY? 166 UNTIL 167 ; 168 169 : TURNRIGHT ( --- ) 170 RTURN_SPEED RTURN_RATE + SETLSPEED 171 RTURN_SPEED RTURN_RATE - SETRSPEED 172 ; 173 174 : TESTTURNRIGHT ( --- ) 175 10 EMIT 176 13 EMIT 177 BEGIN 178 RTURN_SPEED RTURN_RATE + 6 .r 32 EMIT 179 RTURN_SPEED RTURN_RATE - 6 .r 180 13 EMIT 181 KEY? 182 UNTIL 183 ; 184 185 : CALCP ( rdist --- n ) 186 LWALL_TARGET - 187 KP 1000 */ 188 ; 189 190 : CALCD ( rdist --- n ) 191 PREV_RDIST @ - 192 KD 1000 */ 193 ; 194 195 \ turnrate: +ve -> closer to left wall, -ve further from left wall 196 : FOLLOWLEFT ( --- turnrate ) 197 RDIST DUP DUP \ -- rdist rdist rdist 198 CALCP \ -- rdist rdist prop 199 ROT \ -- rdist prop rdist 200 CALCD \ -- rdist prop deriv 201 + \ -- rdist sum 202 SWAP \ -- sum rdist 203 PREV_RDIST ! \ -- 204 ; 205 206 : LINESTEER ( speed turnrate --- ) 207 2DUP 208 + SETLSPEED 209 ROT ROT 210 NEGATE + SETRSPEED 211 ; 212 213 : FOLLOWINIT ( --- ) 214 0 PREV_RDIST ! 215 ; 216 217 \ : FOLLOW ( --- ) 218 \ FOLLOWINIT 219 \ BEGIN 220 \ FRONTWALL? 221 \ IF 222 \ TURNRIGHT 223 \ ELSE 224 \ FOLLOWLEFT LINESTEER 225 \ THEN 226 \ SYNCH \ synchronise to 1ms interrupt edge 227 \ KEY? 228 \ UNTIL 229 \ STOPMOTORS 230 \ ; 231 232 : FRONTDIST ( --- distance ) 233 LDIST ; 234 235 : SELECTSPEED ( distance --- distance speed ) 236 DUP FWALL_FAR < IF FAST_SPEED ELSE SLOW_SPEED THEN ; 237 238 : SELECTMOVE ( speed distance --- ) 239 FWALL_NEAR > IF DROP TURNRIGHT ELSE FOLLOWLEFT LINESTEER THEN ; 240 241 : WALLFOLLOW ( --- ) 242 FOLLOWINIT 243 BEGIN 244 FRONTDIST SELECTSPEED SWAP SELECTMOVE SYNCH 245 KEY? 246 UNTIL 247 STOPMOTORS 248 ; 249