71 #include <propeller.h>
82 #define _waitcnt(x) __builtin_propeller_waitcnt((x), 0)
83 #include "scribbler.h"
86 #define INLINE__ static inline
87 #define Yield__() __asm__ volatile( "" ::: "memory" )
88 #define BitEncode__(X) (32 - __builtin_clz(X))
90 #define INLINE__ static
92 #define waitcnt(n) _waitcnt(n)
93 #define coginit(id, code, par) _coginit((unsigned)(par)>>2, (unsigned)(code)>>2, id)
94 #define cognew(code, par) coginit(0x8, (code), (par))
95 #define cogstop(i) _cogstop(i)
96 INLINE__ int32_t BitEncode__(uint32_t a) {
98 while (a != 0) { a = a>>1; r++; }
103 INLINE__ int32_t Min__(int32_t a, int32_t b) {
return a < b ? a : b; }
104 INLINE__ int32_t Max__(int32_t a, int32_t b) {
return a > b ? a : b; }
105 INLINE__ int32_t Shr__(uint32_t a, uint32_t b) {
return (a>>b); }
106 INLINE__ int32_t Lookdown__(int32_t
x, int32_t b, int32_t a[], int32_t n) { int32_t i, r; r = 0;
for (i = 0; i < n; i++) {
if (a[i] ==
x) { r = i+b;
break; } };
return r; }
108 static uint32_t Sqrt__(uint32_t a) {
110 uint32_t bit = 1<<30;
111 while (bit > a) bit = bit>>2;
115 res = (res>>1) + bit;
116 }
else res = res >> 1;
121 static void scribbler__compute_calibration(
void);
122 static int32_t scribbler__atan2(int32_t y, int32_t
x);
123 static int32_t scribbler__enqueue_tone(int32_t tone_word);
124 static int32_t scribbler__i2c_rd(int32_t acknak);
125 static int32_t scribbler__i2c_wr(int32_t data);
126 static void scribbler__i2c_start(
void);
127 static void scribbler__i2c_stop(
void);
128 static int32_t scribbler__ee_rdblock(int32_t dest_addr, int32_t addr, int32_t size);
129 static int32_t scribbler__ee_wrblock(int32_t src_addr, int32_t addr, int32_t size);
130 static int32_t scribbler__ee_rdbyte(int32_t addr);
131 static int32_t scribbler__ee_wrbyte(int32_t addr, int32_t data);
132 static void scribbler__ee_waddr(int32_t addr);
133 static int32_t scribbler__BQ2495_rdbyte(int32_t addr);
134 static int32_t scribbler__BQ2495_wrbyte(int32_t addr, int32_t data);
135 static void scribbler__BQ2495_waddr(int32_t addr);
137 extern uint8_t _dat_scribbler_[] __asm__(
"..dat_start");
138 #define _tostr__(...) #__VA_ARGS__
139 #define _tostr_(...) _tostr__(__VA_ARGS__)
140 #define _dat_(...) __asm__(_tostr_(__VA_ARGS__) "\n")
141 #define _lbl_(x) (x - _org_)
142 #define _org_ ..dat_start
143 extern int32_t env_det[] __asm__(
"env_det");
144 extern int32_t mic_cyc[] __asm__(
"mic_cyc");
145 extern int32_t mic_dt[] __asm__(
"mic_dt");
146 extern int32_t tone_seq[] __asm__(
"tone_seq");
147 extern int32_t dttime[] __asm__(
"dttime");
148 extern int32_t queue_addr[] __asm__(
"queue_addr");
149 extern int32_t adc_all[] __asm__(
"adc_all");
150 extern int32_t results_addr[] __asm__(
"results_addr");
151 extern int32_t seq_addr[] __asm__(
"seq_addr");
152 extern int32_t motor_driver[] __asm__(
"motor_driver");
153 extern int32_t midler_addr[] __asm__(
"midler_addr");
154 extern uint16_t Adc_sequence[] __asm__(
"Adc_sequence");
155 extern int32_t Timers[] __asm__(
"Timers");
156 extern int32_t Envelope[] __asm__(
"Envelope");
157 extern int32_t Current_x[] __asm__(
"Current_x");
158 extern int32_t Current_y[] __asm__(
"Current_y");
159 extern int32_t Current_w[] __asm__(
"Current_w");
160 extern int32_t Stall_hyst[] __asm__(
"Stall_hyst");
161 extern uint16_t Full_circle[] __asm__(
"Full_circle");
162 extern uint16_t Wheel_space[] __asm__(
"Wheel_space");
163 extern uint16_t Half_circle[] __asm__(
"Half_circle");
164 extern uint16_t Qtr_circle[] __asm__(
"Qtr_circle");
165 extern int32_t Atan_circle[] __asm__(
"Atan_circle");
166 extern uint8_t Light_scale[] __asm__(
"Light_scale");
167 extern uint8_t Line_thld[] __asm__(
"Line_thld");
168 extern uint8_t Obstacle_thld[] __asm__(
"Obstacle_thld");
169 extern uint16_t Motor_cmd[] __asm__(
"Motor_cmd") __attribute__((aligned(4)));
170 extern uint16_t Motor_Rdist[] __asm__("Motor_Rdist") __attribute__((aligned(4)));
171 extern uint16_t Motor_Ldist[] __asm__("Motor_Ldist");
172 extern int32_t Motor_stat[] __asm__("Motor_stat");
173 extern int32_t Path_Rdist[] __asm__("Path_Rdist");
174 extern int32_t Path_Ldist[] __asm__("Path_Ldist");
175 extern int32_t Path_time[] __asm__("Path_time");
176 extern int32_t Path_max_spd[] __asm__("Path_max_spd");
177 extern uint16_t Results[] __asm__("Results");
178 extern uint16_t Tone_queue[] __asm__("Tone_queue");
179 extern uint16_t Tone_enq_ptr[] __asm__("Tone_enq_ptr");
180 extern uint16_t Tone_deq_ptr[] __asm__("Tone_deq_ptr");
181 extern uint8_t Tone_cmd[] __asm__("Tone_cmd");
182 extern uint8_t Tone_sync[] __asm__("Tone_sync");
183 extern uint8_t Tone_voice1[] __asm__("Tone_voice1");
184 extern uint8_t Tone_voice2[] __asm__("Tone_voice2");
185 extern uint8_t In_path[] __asm__("In_path");
186 extern uint8_t Adc_cog[] __asm__("Adc_cog");
187 extern uint8_t Tone_cog[] __asm__("Tone_cog");
188 extern uint8_t Mic_cog[] __asm__("Mic_cog");
189 extern uint8_t Motor_cog[] __asm__("Motor_cog");
190 extern uint8_t Reset_count[] __asm__("Reset_count");
191 extern uint8_t Current_spd[] __asm__("Current_spd");
192 extern uint8_t Model_number[] __asm__("Model_number");
194 _dat_( .compress off );
195 _dat_( ..dat_start: );
199 #define _org_ ..org0001
200 _dat_(..org0001_base = . + 0x0 );
203 _dat_( mov ctra, _lbl_(mic_ctra0) );
204 _dat_( mov dira, _lbl_(mic_dira0) );
205 _dat_( mov frqa, #1 );
207 _dat_( mov _lbl_(mic_bias), _lbl_(mic_dt) );
209 _dat_( shr _lbl_(mic_bias), #1 );
211 _dat_( mov _lbl_(mic_time), cnt );
212 _dat_( add _lbl_(mic_time), _lbl_(mic_dt) );
214 _dat_( mov _lbl_(mic_pamp), phsa );
217 _dat_( mov _lbl_(mic_acc), #0 );
219 _dat_( mov _lbl_(mic_cnt), _lbl_(mic_cyc) );
222 _dat_( jmpret _lbl_(mic_samp_ret), #_lbl_(mic_samp) );
224 _dat_( addabs _lbl_(mic_acc), _lbl_(mic_amp) );
226 _dat_( djnz _lbl_(mic_cnt), #_lbl_(mic_lp) );
228 _dat_( wrlong _lbl_(mic_acc), par );
230 _dat_( jmp #_lbl_(env_lp) );
235 _dat_(
waitcnt _lbl_(mic_time), _lbl_(mic_dt) );
237 _dat_( mov _lbl_(mic_amp), phsa );
239 _dat_( sub _lbl_(mic_amp), _lbl_(mic_pamp) );
241 _dat_( add _lbl_(mic_pamp), _lbl_(mic_amp) );
243 _dat_( sub _lbl_(mic_amp), _lbl_(mic_bias) );
245 _dat_( shl _lbl_(mic_bias), #4 );
246 _dat_( add _lbl_(mic_bias), _lbl_(mic_amp) );
247 _dat_( shr _lbl_(mic_bias), #4 );
248 _dat_( mic_samp_ret: );
251 _dat_( .
long (0 - 0) );
253 _dat_( .
long (0 - 0) );
255 _dat_( .
long (1 << SCRIBBLER_MIC_ADC_OUT) );
257 _dat_( .
long (((9 << $1a) | (SCRIBBLER_MIC_ADC_OUT << 9)) | SCRIBBLER_MIC_ADC_IN));
274 #define _org_ ..org0002
275 _dat_(..org0002_base = . + 0x0 );
278 _dat_( mov ctra, _lbl_(tctra0) );
279 _dat_( mov _lbl_(tacc), _lbl_(queue_addr) );
280 _dat_( add _lbl_(tacc), _lbl_(queue_size) );
281 _dat_( add _lbl_(tacc), _lbl_(queue_size) );
282 _dat_( mov _lbl_(enq_ptr_addr), _lbl_(tacc) );
283 _dat_( add _lbl_(tacc), #2 );
284 _dat_( mov _lbl_(deq_ptr_addr), _lbl_(tacc) );
285 _dat_( add _lbl_(tacc), #2 );
286 _dat_( mov _lbl_(cmd_addr), _lbl_(tacc) );
287 _dat_( add _lbl_(tacc), #1 );
288 _dat_( mov _lbl_(sync_addr), _lbl_(tacc) );
289 _dat_( mov _lbl_(ttime), cnt );
290 _dat_( add _lbl_(ttime), _lbl_(dttime) );
291 _dat_( Tone_seq_get_cmd: );
293 _dat_( jmpret _lbl_(dequeue_ret), #_lbl_(dequeue) );
295 _dat_( mov _lbl_(cmd), _lbl_(tacc) );
297 _dat_( and _lbl_(tacc), _lbl_(data_bits) );
299 _dat_( shr _lbl_(cmd), #$d wz );
301 _dat_( if_z jmp #_lbl_(Tone_seq_tone) );
303 _dat_( cmp _lbl_(cmd), #SCRIBBLER_VOLU wz );
304 _dat_( if_nz jmp #_lbl_(Tone_seq_try_sync) );
306 _dat_( shl _lbl_(tacc), #3 );
308 _dat_( mov _lbl_(volume), _lbl_(tacc) );
310 _dat_( jmp #_lbl_(Tone_seq_get_cmd) );
311 _dat_( Tone_seq_try_sync: );
313 _dat_( cmp _lbl_(cmd), #SCRIBBLER_SYNC wz );
315 _dat_( if_z jmp #_lbl_(Tone_seq_do_sync) );
317 _dat_( cmp _lbl_(cmd), #SCRIBBLER_PAUS wz );
319 _dat_( if_nz jmp #_lbl_(Tone_seq_get_cmd) );
321 _dat_( mov _lbl_(playing), #0 );
322 _dat_( Tone_seq_do_sync: );
324 _dat_( wrbyte _lbl_(tacc), _lbl_(sync_addr) );
326 _dat_( jmp #_lbl_(Tone_seq_get_cmd) );
327 _dat_( Tone_seq_tone: );
329 _dat_( mov _lbl_(duration), _lbl_(tacc) );
331 _dat_( shl _lbl_(tacc), #6 );
332 _dat_( add _lbl_(duration), _lbl_(tacc) );
333 _dat_( shr _lbl_(duration), #1 );
335 _dat_( jmpret _lbl_(dequeue_ret), #_lbl_(dequeue) );
336 _dat_( mov _lbl_(freq1), _lbl_(tacc) );
338 _dat_( shl _lbl_(freq1), #$12 wz );
340 _dat_( shr _lbl_(freq1), #1 );
342 _dat_( if_z mov _lbl_(phase1), #0 );
344 _dat_( mov _lbl_(voice1), _lbl_(tacc) );
345 _dat_( shr _lbl_(voice1), #$e );
347 _dat_( jmpret _lbl_(dequeue_ret), #_lbl_(dequeue) );
348 _dat_( mov _lbl_(freq2), _lbl_(tacc) );
349 _dat_( shl _lbl_(freq2), #$12 wz );
350 _dat_( shr _lbl_(freq2), #1 );
351 _dat_( if_z mov _lbl_(phase2), #0 );
352 _dat_( mov _lbl_(voice2), _lbl_(tacc) );
353 _dat_( shr _lbl_(voice2), #$e );
354 _dat_( or dira, _lbl_(tdira0) );
355 _dat_( Tone_seq_tone_lp: );
357 _dat_( rdbyte _lbl_(tacc), _lbl_(cmd_addr) );
359 _dat_( cmp _lbl_(tacc), #SCRIBBLER_STOP wz );
361 _dat_( if_z jmp #_lbl_(Tone_seq_get_cmd) );
363 _dat_( add _lbl_(phase1), _lbl_(freq1) );
364 _dat_( add _lbl_(phase2), _lbl_(freq2) );
366 _dat_( mov _lbl_(phase), _lbl_(phase1) );
367 _dat_( mov _lbl_(voice), _lbl_(voice1) );
369 _dat_( jmpret _lbl_(get_amp_ret), #_lbl_(get_amp) );
371 _dat_( mov _lbl_(amp), _lbl_(tacc) );
373 _dat_( mov _lbl_(phase), _lbl_(phase2) );
374 _dat_( mov _lbl_(voice), _lbl_(voice2) );
376 _dat_( jmpret _lbl_(get_amp_ret), #_lbl_(get_amp) );
378 _dat_( add _lbl_(amp), _lbl_(tacc) );
380 _dat_( abs _lbl_(tacc), _lbl_(amp) );
381 _dat_( mov _lbl_(taccx), _lbl_(volume) );
382 _dat_( jmpret _lbl_(fmult_ret), #_lbl_(fmult) );
384 _dat_( shl _lbl_(amp), #1 wc );
386 _dat_( mov _lbl_(amp), _lbl_(amp0) );
388 _dat_( sumc _lbl_(amp), _lbl_(tacc) );
390 _dat_(
waitcnt _lbl_(ttime), _lbl_(dttime) );
392 _dat_( mov frqa, _lbl_(amp) );
394 _dat_( djnz _lbl_(duration), #_lbl_(Tone_seq_tone_lp) );
396 _dat_( jmp #_lbl_(Tone_seq_get_cmd) );
400 _dat_( test _lbl_(voice), #2 wc );
401 _dat_( test _lbl_(voice), #1 wz );
403 _dat_( if_c jmp #_lbl_(Get_amp_tri_sin) );
404 _dat_( if_nz jmp #_lbl_(Get_amp_saw) );
405 _dat_( Get_amp_squ: );
407 _dat_( shl _lbl_(phase), #1 wc, nr );
409 _dat_( negc _lbl_(tacc), _lbl_(maxamp) );
411 _dat_( jmp _lbl_(get_amp_ret) );
412 _dat_( Get_amp_saw: );
414 _dat_( mov _lbl_(tacc), _lbl_(phase) );
416 _dat_( add _lbl_(tacc), _lbl_(amp0) );
418 _dat_( sar _lbl_(tacc), #1 );
420 _dat_( jmp _lbl_(get_amp_ret) );
421 _dat_( Get_amp_tri_sin: );
422 _dat_( if_nz jmp #_lbl_(Get_amp_sin) );
423 _dat_( Get_amp_tri: );
425 _dat_( abs _lbl_(tacc), _lbl_(phase) );
427 _dat_( sub _lbl_(tacc), _lbl_(maxamp) );
429 _dat_( jmp _lbl_(get_amp_ret) );
430 _dat_( Get_amp_sin: );
432 _dat_( shr _lbl_(phase), #($20 - $d) );
434 _dat_( test _lbl_(phase), _lbl_(_0x1000) wz );
436 _dat_( test _lbl_(phase), _lbl_(_0x0800) wc );
438 _dat_( negc _lbl_(phase), _lbl_(phase) );
440 _dat_( or _lbl_(phase), _lbl_(_0x7000) );
442 _dat_( shl _lbl_(phase), #1 );
444 _dat_( rdword _lbl_(tacc), _lbl_(phase) );
446 _dat_( negnz _lbl_(tacc), _lbl_(tacc) );
448 _dat_( shl _lbl_(tacc), #$e );
449 _dat_( get_amp_ret: );
454 _dat_( rdbyte _lbl_(tacc), _lbl_(cmd_addr) );
456 _dat_( cmp _lbl_(tacc), #SCRIBBLER_PAUSE wz );
458 _dat_( if_nz cmp _lbl_(tacc), #SCRIBBLER_STOP wz );
460 _dat_( if_z mov _lbl_(playing), #0 );
462 _dat_( if_z jmp #_lbl_(Dequeue_zapcmd) );
464 _dat_( cmp _lbl_(tacc), #SCRIBBLER_PLAY wz );
466 _dat_( if_z mov _lbl_(playing), #1 );
467 _dat_( Dequeue_zapcmd: );
469 _dat_( if_z wrbyte _lbl_(zero), _lbl_(cmd_addr) );
471 _dat_( test _lbl_(playing), _lbl_(playing) wz );
473 _dat_( if_z jmp #_lbl_(Dequeue_decay) );
475 _dat_( rdword _lbl_(enq_ptr), _lbl_(enq_ptr_addr) );
476 _dat_( rdword _lbl_(deq_ptr), _lbl_(deq_ptr_addr) );
478 _dat_( cmp _lbl_(enq_ptr), _lbl_(deq_ptr) wz );
480 _dat_( if_nz jmp #_lbl_(Dequeue_get_it) );
481 _dat_( Dequeue_decay: );
483 _dat_( cmpsub frqa, #$1ff wc );
485 _dat_( if_nc andn dira, _lbl_(tdira0) );
487 _dat_( mov _lbl_(ttime), cnt );
488 _dat_( add _lbl_(ttime), _lbl_(dttime) );
490 _dat_( jmp #_lbl_(dequeue) );
491 _dat_( Dequeue_get_it: );
492 _dat_( shl _lbl_(deq_ptr), #1 );
494 _dat_( add _lbl_(deq_ptr), _lbl_(queue_addr) );
496 _dat_( rdword _lbl_(tacc), _lbl_(deq_ptr) );
498 _dat_( sub _lbl_(deq_ptr), _lbl_(queue_addr) );
499 _dat_( shr _lbl_(deq_ptr), #1 );
501 _dat_( add _lbl_(deq_ptr), #1 );
503 _dat_( cmpsub _lbl_(deq_ptr), _lbl_(queue_size) );
505 _dat_( wrword _lbl_(deq_ptr), _lbl_(deq_ptr_addr) );
506 _dat_( dequeue_ret: );
516 _dat_( mov _lbl_(t0), #0 );
518 _dat_( mov _lbl_(t1), #$20 );
520 _dat_( shr _lbl_(tacc), #1 wc );
521 _dat_( Fmult_loop: );
523 _dat_( if_c add _lbl_(t0), _lbl_(taccx) wc );
525 _dat_( rcr _lbl_(t0), #1 wc );
526 _dat_( rcr _lbl_(tacc), #1 wc );
528 _dat_( djnz _lbl_(t1), #_lbl_(Fmult_loop) );
530 _dat_( shr _lbl_(tacc), #$10 );
531 _dat_( shl _lbl_(t0), #$10 );
532 _dat_( or _lbl_(tacc), _lbl_(t0) );
537 _dat_( .
long (0 - 0) );
538 _dat_( queue_addr: );
539 _dat_( .
long (0 - 0) );
541 _dat_( .
long ((6 << $1a) | SCRIBBLER_SPEAKER) );
543 _dat_( .
long (1 << SCRIBBLER_SPEAKER) );
547 _dat_( .
long $80000000 );
549 _dat_( .
long $3fffffff );
551 _dat_( .
long $1fff );
553 _dat_( .
long $1000 );
557 _dat_( .
long $7000 );
559 _dat_( .
long $8000 );
562 _dat_( queue_size: );
563 _dat_( .
long SCRIBBLER__TONE_Q_SIZE );
564 _dat_( enq_ptr_addr: );
567 _dat_( deq_ptr_addr: );
613 #define _org_ ..org0003
614 _dat_(..org0003_base = . + 0x0 );
617 _dat_( mov ctra, _lbl_(ctra0) );
619 _dat_( mov dira, _lbl_(dira0) );
620 _dat_( mov frqa, #1 );
621 _dat_( Adc_all_ee_setup: );
624 _dat_( mov _lbl_(acc), par );
626 _dat_( shr _lbl_(acc), #2 );
628 _dat_( tjnz _lbl_(acc), #_lbl_(Adc_all_ee_setup_s3) );
630 _dat_( mov _lbl_(ee_addr_cmd), #$a2 );
632 _dat_( mov _lbl_(ee_offset), _lbl_(s2_ee_offset) );
633 _dat_( jmp #_lbl_(Adc_all_cal_start) );
634 _dat_( Adc_all_ee_setup_s3: );
636 _dat_( mov _lbl_(ee_addr_cmd), #$a0 );
638 _dat_( mov _lbl_(ee_offset), _lbl_(s3_ee_offset) );
639 _dat_( Adc_all_cal_start: );
642 _dat_( movd _lbl_(Adc_all_got_it), #(_lbl_(intvl3)/4) );
643 _dat_( movd (_lbl_(Adc_all_got_it) + 4), #(_lbl_(loresult3)/4));
644 _dat_( movs _lbl_(Adc_all_set_mux), #SCRIBBLER__MUX_VDD );
645 _dat_( mov _lbl_(
count), #2 );
646 _dat_( mov _lbl_(soak_time), _lbl_(one_ms) );
647 _dat_( Adc_all_calibrate: );
649 _dat_( mov _lbl_(intvl), _lbl_(intvl0) );
651 _dat_( mov _lbl_(dintvl), _lbl_(intvl0) );
652 _dat_( shr _lbl_(dintvl), #1 );
653 _dat_( Adc_all_searchlp: );
655 _dat_( mov _lbl_(mux_addr), #SCRIBBLER__MUX_VSS );
657 _dat_( jmpret _lbl_(adc_ret), #_lbl_(adc) );
659 _dat_( mov _lbl_(loresult), _lbl_(acc) );
660 _dat_( Adc_all_set_mux: );
662 _dat_( mov _lbl_(mux_addr), #(0 - 0) );
664 _dat_( jmpret _lbl_(adc_ret), #_lbl_(adc) );
666 _dat_( sub _lbl_(acc), _lbl_(loresult) );
668 _dat_( cmps _lbl_(acc), #$ff wz, wc );
670 _dat_( if_z jmp #_lbl_(Adc_all_got_it) );
672 _dat_( sumnc _lbl_(intvl), _lbl_(dintvl) );
674 _dat_( shr _lbl_(dintvl), #1 wz );
676 _dat_( if_nz jmp #_lbl_(Adc_all_searchlp) );
677 _dat_( Adc_all_got_it: );
679 _dat_( mov (0 - 0), _lbl_(intvl) );
680 _dat_( mov (0 - 0), _lbl_(loresult) );
682 _dat_( movd _lbl_(Adc_all_got_it), #(_lbl_(intvl5)/4) );
683 _dat_( movd (_lbl_(Adc_all_got_it) + 4), #(_lbl_(loresult5)/4));
684 _dat_( movs _lbl_(Adc_all_set_mux), #SCRIBBLER__MUX_5V );
686 _dat_( djnz _lbl_(
count), #_lbl_(Adc_all_calibrate) );
689 _dat_( mov _lbl_(loop_timer), cnt );
691 _dat_( add _lbl_(loop_timer), _lbl_(one_ms) );
694 _dat_(
waitcnt _lbl_(loop_timer), _lbl_(one_ms) );
696 _dat_( mov _lbl_(results_ptr), _lbl_(results_addr) );
697 _dat_( add _lbl_(results_ptr), #(SCRIBBLER_TIMER * 2) );
698 _dat_( rdlong _lbl_(acc), _lbl_(results_ptr) );
699 _dat_( add _lbl_(acc), #1 );
700 _dat_( wrlong _lbl_(acc), _lbl_(results_ptr) );
702 _dat_( mov _lbl_(seq_ptr), _lbl_(seq_addr) );
704 _dat_( add _lbl_(loop_ctr), #1 );
705 _dat_( Main_lp_sample_lp: );
707 _dat_( rdword _lbl_(acc), _lbl_(seq_ptr) wz );
709 _dat_( if_z jmp #_lbl_(Main_lp_do_button) );
711 _dat_( add _lbl_(seq_ptr), #2 );
713 _dat_( mov _lbl_(mux_addr), _lbl_(acc) );
714 _dat_( shr _lbl_(mux_addr), #$c );
716 _dat_( mov _lbl_(results_ptr), _lbl_(acc) );
717 _dat_( shr _lbl_(results_ptr), #7 );
718 _dat_( and _lbl_(results_ptr), #$1e );
719 _dat_( add _lbl_(results_ptr), _lbl_(results_addr) );
721 _dat_( mov _lbl_(
count), _lbl_(acc) );
722 _dat_( shr _lbl_(
count), #4 );
723 _dat_( and _lbl_(
count), #$f );
724 _dat_( mov _lbl_(soak_time), #$50 );
725 _dat_( shl _lbl_(soak_time), _lbl_(
count) );
727 _dat_( mov _lbl_(filter), _lbl_(acc) );
728 _dat_( shr _lbl_(filter), #1 );
729 _dat_( and _lbl_(filter), #7 );
731 _dat_( jmpret _lbl_(do_adc_ret), #_lbl_(do_adc) );
733 _dat_( cmp _lbl_(mux_addr), #SCRIBBLER__MUX_IDLER wz);
735 _dat_( if_nz jmp #_lbl_(Main_lp_not_idler) );
737 _dat_( mov _lbl_(accx), _lbl_(acc) );
739 _dat_( or outa, _lbl_(idler_on) );
741 _dat_( andn _lbl_(acc), #1 );
743 _dat_( jmpret _lbl_(do_adc_ret), #_lbl_(do_adc) );
745 _dat_( andn outa, _lbl_(idler_on) );
747 _dat_( sub _lbl_(acc), _lbl_(accx) );
749 _dat_( mins _lbl_(acc), #0 );
750 _dat_( Main_lp_not_idler: );
752 _dat_( rdword _lbl_(
count), _lbl_(results_ptr) );
754 _dat_( mov _lbl_(accx), _lbl_(
count) );
756 _dat_( shr _lbl_(accx), _lbl_(filter) );
758 _dat_( sub _lbl_(
count), _lbl_(accx) );
760 _dat_( shl _lbl_(acc), #8 );
762 _dat_( shr _lbl_(acc), _lbl_(filter) );
764 _dat_( add _lbl_(
count), _lbl_(acc) );
766 _dat_( wrword _lbl_(
count), _lbl_(results_ptr) );
768 _dat_( if_nz jmp #_lbl_(Main_lp_sample_lp) );
770 _dat_( shr _lbl_(
count), #8 );
772 _dat_( test _lbl_(idler_state), #1 wz );
774 _dat_( if_nz mov _lbl_(acc), #$a );
776 _dat_( if_z mov _lbl_(acc), #$f );
778 _dat_( cmp _lbl_(
count), _lbl_(acc) wc );
780 _dat_( muxnc _lbl_(idler_state), #1 );
782 _dat_( if_z_eq_c jmp #_lbl_(Main_lp_sample_lp) );
784 _dat_( mov _lbl_(results_ptr), _lbl_(results_addr) );
785 _dat_( add _lbl_(results_ptr), #(SCRIBBLER_CNT_IDLER * 2));
787 _dat_( rdword _lbl_(
count), _lbl_(results_ptr) );
789 _dat_( add _lbl_(
count), #1 );
791 _dat_( wrword _lbl_(
count), _lbl_(results_ptr) );
793 _dat_( jmp #_lbl_(Main_lp_sample_lp) );
794 _dat_( Main_lp_do_button: );
796 _dat_( mov _lbl_(results_ptr), _lbl_(results_addr) );
797 _dat_( add _lbl_(results_ptr), #((SCRIBBLER_BUTTON_CNT * 2) + 1));
799 _dat_( rdbyte _lbl_(btn_flgs), _lbl_(results_ptr) );
801 _dat_( test _lbl_(btn_mask), ina wc );
803 _dat_( if_nc mov _lbl_(btn_timer), _lbl_(one_second) );
805 _dat_( rcl _lbl_(btn_shift), #1 wz );
807 _dat_( test _lbl_(btn_cnt), #1 wc );
809 _dat_( if_c add _lbl_(btn_shift), #1 wz, nr );
811 _dat_( if_z add _lbl_(btn_cnt), #1 );
813 _dat_( mov _lbl_(accx), _lbl_(btn_cnt) );
815 _dat_( shr _lbl_(accx), #1 wz );
817 _dat_( max _lbl_(accx), #8 );
819 _dat_( cmpsub _lbl_(btn_timer), #1 wc );
821 _dat_( if_z_or_c jmp #_lbl_(Main_lp_do_leds) );
823 _dat_( sub _lbl_(results_ptr), #1 );
825 _dat_( wrbyte _lbl_(accx), _lbl_(results_ptr) );
827 _dat_( mov _lbl_(btn_cnt), #0 );
829 _dat_( test _lbl_(btn_flgs), #SCRIBBLER_RST_ENA wc );
831 _dat_( if_nc jmp #_lbl_(Main_lp_do_leds) );
833 _dat_(
cogid _lbl_(acc) );
835 _dat_( mov _lbl_(
count), #7 );
836 _dat_( Main_lp_stop_cogs: );
838 _dat_( cmp _lbl_(acc), _lbl_(
count) wz );
841 _dat_( sub _lbl_(
count), #1 wc );
842 _dat_( if_nc jmp #_lbl_(Main_lp_stop_cogs) );
844 _dat_( mov _lbl_(ee_addr), #SCRIBBLER_EE_RESET_CNT );
846 _dat_( jmpret _lbl_(i2c_waddr_ret), #_lbl_(i2c_waddr) );
848 _dat_( mov _lbl_(acc), _lbl_(accx) );
850 _dat_( jmpret _lbl_(i2c_wr_ret), #_lbl_(i2c_wr) );
852 _dat_( jmpret _lbl_(i2c_stop_ret), #_lbl_(
i2c_stop) );
854 _dat_( mov _lbl_(dintvl), #0 );
855 _dat_( jmpret _lbl_(show_leds_ret), #_lbl_(show_leds) );
857 _dat_(
clkset _lbl_(reeboot) );
858 _dat_( Main_lp_do_leds: );
860 _dat_( test _lbl_(btn_flgs), #SCRIBBLER_LED_ENA wc );
862 _dat_( if_z_or_nc jmp #_lbl_(Main_lp_reg_led) );
864 _dat_( mov _lbl_(acc), _lbl_(led_seq) );
866 _dat_( shr _lbl_(acc), _lbl_(accx) );
867 _dat_( shr _lbl_(acc), _lbl_(accx) );
869 _dat_( mov _lbl_(dintvl), #0 );
871 _dat_( mov _lbl_(
count), #6 );
873 _dat_( mov _lbl_(intvl), #$10 );
874 _dat_( Main_lp_led_seq_lp: );
876 _dat_( shr _lbl_(acc), #1 wc );
878 _dat_( muxc _lbl_(dintvl), _lbl_(intvl) );
880 _dat_( test _lbl_(intvl), #$20 wc );
881 _dat_( and _lbl_(intvl), #$1f );
882 _dat_( rcl _lbl_(intvl), #1 );
884 _dat_( djnz _lbl_(
count), #_lbl_(Main_lp_led_seq_lp) );
886 _dat_( test _lbl_(loop_ctr), #$40 wc );
888 _dat_( test _lbl_(btn_flgs), #SCRIBBLER_RST_ENA wz );
890 _dat_( if_nz muxc _lbl_(dintvl), #$80 );
892 _dat_( jmp #_lbl_(Main_lp_show_leds) );
893 _dat_( Main_lp_reg_led: );
895 _dat_( mov _lbl_(results_ptr), _lbl_(results_addr) );
896 _dat_( add _lbl_(results_ptr), #(SCRIBBLER_LED_BYTES * 2));
898 _dat_( mov _lbl_(
count), #4 );
900 _dat_( mov _lbl_(intvl), _lbl_(loop_ctr) );
901 _dat_( and _lbl_(intvl), #$f );
902 _dat_( add _lbl_(intvl), #1 );
903 _dat_( Main_lp_led_lp: );
905 _dat_( rdbyte _lbl_(acc), _lbl_(results_ptr) );
907 _dat_( add _lbl_(results_ptr), #1 );
909 _dat_( mov _lbl_(accx), _lbl_(acc) );
910 _dat_( shr _lbl_(accx), #4 );
912 _dat_( and _lbl_(acc), #$f );
914 _dat_( mov _lbl_(soak_time), _lbl_(acc) );
915 _dat_( add _lbl_(soak_time), _lbl_(accx) );
917 _dat_( cmp _lbl_(soak_time), #$10 wc );
919 _dat_( if_nc test _lbl_(loop_ctr), #$100 wz );
921 _dat_( if_nc_and_z mov _lbl_(acc), #0 );
923 _dat_( if_nc_and_nz mov _lbl_(accx), #0 );
925 _dat_( cmp _lbl_(acc), #1 wz );
926 _dat_( if_z mov _lbl_(acc), #0 );
927 _dat_( cmp _lbl_(accx), #1 wz );
928 _dat_( if_z mov _lbl_(accx), #0 );
930 _dat_( mov _lbl_(soak_time), _lbl_(acc) );
931 _dat_( add _lbl_(soak_time), _lbl_(accx) );
933 _dat_( shl _lbl_(dintvl), #2 );
935 _dat_( cmp _lbl_(soak_time), _lbl_(intvl) wc );
937 _dat_( if_c jmp #_lbl_(Main_lp_next_led) );
939 _dat_( cmp _lbl_(acc), _lbl_(intvl) wc );
941 _dat_( if_nc or _lbl_(dintvl), #1 );
943 _dat_( if_c or _lbl_(dintvl), #2 );
944 _dat_( Main_lp_next_led: );
946 _dat_( djnz _lbl_(
count), #_lbl_(Main_lp_led_lp) );
947 _dat_( Main_lp_show_leds: );
948 _dat_( jmpret _lbl_(show_leds_ret), #_lbl_(show_leds) );
949 _dat_( jmp #_lbl_(main_lp) );
953 _dat_( mov _lbl_(
count), #8 );
954 _dat_( Show_leds_send_lp: );
956 _dat_( shr _lbl_(dintvl), #1 wc );
958 _dat_( muxc outa, #(1 << SCRIBBLER_LED_DATA) );
960 _dat_( or outa, #(1 << SCRIBBLER_LED_CLK) );
961 _dat_( andn outa, #(1 << SCRIBBLER_LED_CLK) );
963 _dat_( djnz _lbl_(
count), #_lbl_(Show_leds_send_lp) );
965 _dat_( andn outa, #(1 << SCRIBBLER_LED_DATA) );
966 _dat_( show_leds_ret: );
971 _dat_( test _lbl_(acc), #1 wc );
973 _dat_( if_nc mov _lbl_(loresult), _lbl_(loresult3) );
974 _dat_( if_nc mov _lbl_(intvl), _lbl_(intvl3) );
976 _dat_( if_c mov _lbl_(loresult), _lbl_(loresult5) );
977 _dat_( if_c mov _lbl_(intvl), _lbl_(intvl5) );
979 _dat_( test (. - ..org0003), #1 wc );
981 _dat_( jmp #_lbl_(_adc) );
985 _dat_( test (. - ..org0003), #0 wc );
988 _dat_( shl _lbl_(mux_addr), #SCRIBBLER_MUX0 );
990 _dat_( mov _lbl_(acc), outa );
992 _dat_( andn _lbl_(acc), _lbl_(mux_mask) );
994 _dat_( or _lbl_(acc), _lbl_(mux_addr) );
996 _dat_( mov outa, _lbl_(acc) );
998 _dat_( shr _lbl_(mux_addr), #SCRIBBLER_MUX0 );
1000 _dat_( mov _lbl_(adc_timer), cnt );
1002 _dat_( add _lbl_(adc_timer), _lbl_(soak_time) );
1004 _dat_(
waitcnt _lbl_(adc_timer), _lbl_(intvl) );
1006 _dat_( neg _lbl_(acc), phsa );
1008 _dat_(
waitcnt _lbl_(adc_timer), #0 );
1010 _dat_( add _lbl_(acc), phsa );
1013 _dat_( if_c sub _lbl_(acc), _lbl_(loresult) );
1015 _dat_( if_c mins _lbl_(acc), #0 );
1017 _dat_( if_c maxs _lbl_(acc), #$ff );
1019 _dat_( do_adc_ret: );
1022 _dat_( i2c_waddr: );
1026 _dat_( add _lbl_(ee_addr), _lbl_(ee_offset) );
1028 _dat_( jmpret _lbl_(i2c_start_ret), #_lbl_(
i2c_start) );
1030 _dat_( mov _lbl_(acc), _lbl_(ee_addr_cmd) );
1032 _dat_( jmpret _lbl_(i2c_wr_ret), #_lbl_(i2c_wr) );
1034 _dat_( if_c jmp #_lbl_(i2c_waddr) );
1036 _dat_( mov _lbl_(acc), _lbl_(ee_addr) );
1037 _dat_( shr _lbl_(acc), #8 );
1039 _dat_( jmpret _lbl_(i2c_wr_ret), #_lbl_(i2c_wr) );
1041 _dat_( mov _lbl_(acc), _lbl_(ee_addr) );
1043 _dat_( jmpret _lbl_(i2c_wr_ret), #_lbl_(i2c_wr) );
1044 _dat_( i2c_waddr_ret: );
1047 _dat_( i2c_rd_nak: );
1050 _dat_( test (. - ..org0003), #1 wc );
1051 _dat_( jmp #_lbl_(i2c_rd) );
1052 _dat_( i2c_rd_ack: );
1055 _dat_( test (. - ..org0003), #0 wc );
1059 _dat_( mov _lbl_(acc), #$ff );
1060 _dat_( jmp #_lbl_(i2c_rd_wr) );
1064 _dat_( test (. - ..org0003), #1 wc );
1065 _dat_( i2c_rd_wr: );
1068 _dat_( mov _lbl_(
count), #9 );
1070 _dat_( rcl _lbl_(acc), #$18 );
1071 _dat_( I2c_rd_wr_wrlp: );
1073 _dat_( rcl _lbl_(acc), #1 wc );
1075 _dat_( muxnc dira, _lbl_(sda_pin) );
1077 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1079 _dat_( or outa, _lbl_(scl_pin) );
1081 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1083 _dat_( test _lbl_(sda_pin), ina wc );
1085 _dat_( andn outa, _lbl_(scl_pin) );
1087 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1089 _dat_( djnz _lbl_(
count), #_lbl_(I2c_rd_wr_wrlp) );
1091 _dat_( and _lbl_(acc), #$ff );
1092 _dat_( i2c_rd_ack_ret: );
1093 _dat_( i2c_rd_nak_ret: );
1094 _dat_( i2c_rd_ret: );
1095 _dat_( i2c_wr_ret: );
1100 _dat_( andn dira, _lbl_(sda_pin) );
1102 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1104 _dat_( or outa, _lbl_(scl_pin) );
1105 _dat_( or dira, _lbl_(scl_pin) );
1107 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1109 _dat_( or dira, _lbl_(sda_pin) );
1111 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1113 _dat_( andn outa, _lbl_(scl_pin) );
1115 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1116 _dat_( i2c_start_ret: );
1121 _dat_( andn outa, _lbl_(scl_pin) );
1123 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1125 _dat_( or dira, _lbl_(sda_pin) );
1127 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1129 _dat_( or outa, _lbl_(scl_pin) );
1131 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1133 _dat_( andn dira, _lbl_(sda_pin) );
1135 _dat_( jmpret _lbl_(delay650_ret), #_lbl_(delay650) );
1137 _dat_( andn dira, _lbl_(scl_pin) );
1138 _dat_( i2c_stop_ret: );
1143 _dat_( mov _lbl_(adc_timer), #$34 );
1144 _dat_( add _lbl_(adc_timer), cnt );
1145 _dat_(
waitcnt _lbl_(adc_timer), #0 );
1146 _dat_( delay650_ret: );
1150 _dat_( .
long (((9 << $1a) | (SCRIBBLER__MUX_ADC_OUT << 9)) | SCRIBBLER__MUX_ADC_IN));
1152 _dat_( .
long (((((1 << SCRIBBLER__MUX_ADC_OUT) | ($f << SCRIBBLER_MUX0)) | (1 << SCRIBBLER_IDLER_TX)) | (1 << SCRIBBLER_LED_DATA)) | (1 << SCRIBBLER_LED_CLK)));
1155 _dat_( .
long (1 << SCRIBBLER_IDLER_TX) );
1158 _dat_( .
long ($f << SCRIBBLER_MUX0) );
1160 _dat_( .
long (1 << SCRIBBLER_SCL) );
1162 _dat_( .
long (1 << SCRIBBLER_SDA) );
1165 _dat_( .
long $800 );
1168 _dat_( .
long $13880 );
1170 _dat_( one_second: );
1171 _dat_( .
long $3e8 );
1177 _dat_( .
long (1 << SCRIBBLER_BUTTON) );
1182 _dat_( btn_shift: );
1185 _dat_( btn_timer: );
1188 _dat_( idler_state: );
1191 _dat_( results_addr: );
1192 _dat_( .
long (0 - 0) );
1195 _dat_( .
long (0 - 0) );
1198 _dat_( .
long $2a540 );
1200 _dat_( s2_ee_offset: );
1203 _dat_( s3_ee_offset: );
1204 _dat_( .
long $8000 );
1213 _dat_( results_ptr: );
1216 _dat_( soak_time: );
1222 _dat_( loop_timer: );
1225 _dat_( adc_timer: );
1237 _dat_( loresult3: );
1243 _dat_( loresult5: );
1267 _dat_( ee_offset: );
1270 _dat_( ee_addr_cmd: );
1276 #define _org_ ..org0004
1277 _dat_(..org0004_base = . + 0x0 );
1278 _dat_( motor_driver: );
1280 _dat_( mov dira, _lbl_(mdira0) );
1282 _dat_( mov frqa, #1 );
1283 _dat_( mov frqb, #1 );
1285 _dat_( mov _lbl_(mpar_addr), par );
1286 _dat_( add _lbl_(mpar_addr), #4 );
1288 _dat_( mov _lbl_(mstat_addr), par );
1289 _dat_( add _lbl_(mstat_addr), #8 );
1291 _dat_( mov _lbl_(mtime), cnt );
1292 _dat_( add _lbl_(mtime), _lbl_(mdt) );
1294 _dat_( test _lbl_(right_enc_bit), ina wc );
1296 _dat_( rcl _lbl_(right_enc), #1 );
1298 _dat_( test _lbl_(left_enc_bit), ina wc );
1299 _dat_( rcl _lbl_(left_enc), #1 );
1301 _dat_( mov _lbl_(nominal_pwm), #0 );
1302 _dat_( jmpret _lbl_(clr_stat_ret), #_lbl_(clr_stat) );
1304 _dat_( wrlong _lbl_(mzero), par );
1305 _dat_( Motor_driver_main_lp: );
1307 _dat_( rdlong _lbl_(mcmd), par wz );
1309 _dat_( if_nz jmp #_lbl_(Motor_driver_do_cmd) );
1310 _dat_( Motor_driver_stop: );
1312 _dat_( mov ctra, #0 );
1313 _dat_( mov ctrb, #0 );
1315 _dat_( jmpret _lbl_(clr_stat_ret), #_lbl_(clr_stat) );
1316 _dat_( Motor_driver_chk_cmd: );
1318 _dat_( rdlong _lbl_(mcmd), par wz );
1320 _dat_( if_z jmp #_lbl_(Motor_driver_chk_cmd) );
1321 _dat_( Motor_driver_do_cmd: );
1323 _dat_( mov _lbl_(motor_timer), _lbl_(mcmd) );
1325 _dat_( shr _lbl_(motor_timer), #$10 wz );
1327 _dat_( if_nz muxnz _lbl_(mcmd), #SCRIBBLER_MOT_TIMED );
1329 _dat_( mov _lbl_(max_vel), _lbl_(mcmd) );
1330 _dat_( shr _lbl_(max_vel), #8 );
1331 _dat_( and _lbl_(max_vel), #$f );
1333 _dat_( mov _lbl_(end_vel), _lbl_(mcmd) );
1334 _dat_( shr _lbl_(end_vel), #4 );
1335 _dat_( and _lbl_(end_vel), #$f );
1337 _dat_( min _lbl_(end_vel), #8 );
1339 _dat_( and _lbl_(mcmd), #$f );
1341 _dat_( rdlong _lbl_(right_dist), _lbl_(mpar_addr) wz );
1343 _dat_( wrlong _lbl_(mzero), par );
1345 _dat_( if_z jmp #_lbl_(Motor_driver_stop) );
1347 _dat_( mov _lbl_(maccx), outa );
1349 _dat_( mov _lbl_(left_dist), _lbl_(right_dist) );
1350 _dat_( sar _lbl_(left_dist), #$10 );
1352 _dat_( shl _lbl_(right_dist), #$10 );
1353 _dat_( sar _lbl_(right_dist), #$10 );
1355 _dat_( abs _lbl_(right_dist), _lbl_(right_dist) wc );
1357 _dat_( if_nc xor _lbl_(maccx), _lbl_(right_dir_bit) );
1359 _dat_( shl _lbl_(right_dist), #1 );
1361 _dat_( min _lbl_(right_dist), #1 );
1363 _dat_( muxnc outa, _lbl_(right_dir_bit) );
1365 _dat_( abs _lbl_(left_dist), _lbl_(left_dist) wc );
1366 _dat_( if_nc xor _lbl_(maccx), _lbl_(left_dir_bit) );
1367 _dat_( shl _lbl_(left_dist), #1 );
1368 _dat_( min _lbl_(left_dist), #1 );
1369 _dat_( muxnc outa, _lbl_(left_dir_bit) );
1371 _dat_( test _lbl_(maccx), _lbl_(both_dir_bits) wz );
1373 _dat_( if_nz jmpret _lbl_(rst_vel_ret), #_lbl_(rst_vel) );
1375 _dat_( mov _lbl_(max_dist), _lbl_(left_dist) );
1376 _dat_( min _lbl_(max_dist), _lbl_(right_dist) );
1378 _dat_( mov _lbl_(ramp_count), #0 );
1380 _dat_( mov _lbl_(macc), _lbl_(left_dist) );
1381 _dat_( mov _lbl_(maccx), _lbl_(right_dist) );
1383 _dat_( jmpret _lbl_(umult_ret), #_lbl_(umult) );
1385 _dat_( mov _lbl_(right_count), _lbl_(macc) );
1386 _dat_( mov _lbl_(left_count), _lbl_(macc) );
1388 _dat_( cmp _lbl_(left_dist), _lbl_(right_dist) wc );
1390 _dat_( if_c mov _lbl_(right_dom), #1 );
1391 _dat_( if_c mov _lbl_(left_dom), #0 );
1393 _dat_( if_nc mov _lbl_(left_dom), #1 );
1394 _dat_( if_nc mov _lbl_(right_dom), #0 );
1396 _dat_( mov phsa, #0 );
1397 _dat_( mov phsb, #0 );
1399 _dat_( mov ctra, _lbl_(mctra0) );
1400 _dat_( mov ctrb, _lbl_(mctrb0) );
1402 _dat_( mov _lbl_(mtime), cnt );
1403 _dat_( add _lbl_(mtime), _lbl_(mdt) );
1405 _dat_( mov _lbl_(mdtimeout), _lbl_(mdto) );
1407 _dat_( mov _lbl_(mot_stat), #SCRIBBLER_MOT_RUNNING );
1408 _dat_( jmpret _lbl_(put_stat_ret), #_lbl_(put_stat) );
1410 _dat_( rdword _lbl_(idler_cnt), _lbl_(midler_addr) );
1411 _dat_( Motor_driver_motor_lp: );
1412 _dat_( tjnz _lbl_(stat_ctr), #_lbl_(Motor_driver_chk_spd));
1413 _dat_( mov _lbl_(left_vel), #0 );
1414 _dat_( mov _lbl_(right_vel), #0 );
1415 _dat_( mov _lbl_(stat_ctr), #5 );
1416 _dat_( Motor_driver_chk_spd: );
1417 _dat_( tjnz _lbl_(motor_ctr), #_lbl_(Motor_driver_go) );
1419 _dat_( mov _lbl_(motor_ctr), #$190 );
1421 _dat_( mov _lbl_(vel_ctr), #0 );
1422 _dat_( Motor_driver_go: );
1424 _dat_( rdlong _lbl_(macc), par );
1426 _dat_( test _lbl_(macc), #SCRIBBLER_MOT_IMM wz );
1428 _dat_( if_nz jmp #_lbl_(Motor_driver_chk_cmd) );
1430 _dat_( mov _lbl_(targ_vel), #$f );
1432 _dat_( test _lbl_(mcmd), #SCRIBBLER_MOT_TIMED wz );
1434 _dat_( if_z jmp #_lbl_(Motor_driver_timeout_ok) );
1436 _dat_( djnz _lbl_(mdtimeout), #_lbl_(Motor_driver_timeout_ok));
1438 _dat_( mov _lbl_(mdtimeout), _lbl_(mdto) );
1440 _dat_( sub _lbl_(motor_timer), #1 wz );
1442 _dat_( if_z jmp #_lbl_(Motor_driver_main_lp) );
1444 _dat_( mov _lbl_(macc), _lbl_(motor_timer) );
1445 _dat_( shr _lbl_(macc), #4 );
1446 _dat_( max _lbl_(targ_vel), _lbl_(macc) );
1447 _dat_( Motor_driver_timeout_ok: );
1449 _dat_( test _lbl_(mcmd), #SCRIBBLER_MOT_CONT wz );
1451 _dat_( if_z jmp #_lbl_(Motor_driver_ramp_down) );
1453 _dat_( cmp _lbl_(right_count), _lbl_(top_dist) wz, wc);
1454 _dat_( if_z_or_c cmp _lbl_(left_count), _lbl_(top_dist) wz, wc);
1456 _dat_( if_z_or_c add _lbl_(right_count), _lbl_(top_dist) );
1457 _dat_( if_z_or_c add _lbl_(left_count), _lbl_(top_dist) );
1459 _dat_( jmp #_lbl_(Motor_driver_ramp_up) );
1460 _dat_( Motor_driver_ramp_down: );
1462 _dat_( mov _lbl_(macc), _lbl_(max_dist) );
1463 _dat_( sub _lbl_(macc), _lbl_(ramp_count) );
1465 _dat_( shr _lbl_(macc), #3 );
1467 _dat_( min _lbl_(macc), _lbl_(end_vel) );
1469 _dat_( max _lbl_(targ_vel), _lbl_(macc) );
1470 _dat_( Motor_driver_ramp_up: );
1472 _dat_( mov _lbl_(macc), _lbl_(ramp_count) );
1474 _dat_( shr _lbl_(macc), #2 );
1475 _dat_( add _lbl_(cur_vel), #1 );
1477 _dat_( min _lbl_(macc), _lbl_(cur_vel) );
1478 _dat_( sub _lbl_(cur_vel), #1 );
1480 _dat_( max _lbl_(targ_vel), _lbl_(macc) );
1482 _dat_( max _lbl_(targ_vel), _lbl_(max_vel) );
1484 _dat_( min _lbl_(targ_vel), #2 );
1485 _dat_( Motor_driver_cont_ok: );
1487 _dat_( test _lbl_(right_enc_bit), ina wc );
1489 _dat_( rcl _lbl_(right_enc), #1 );
1491 _dat_( test _lbl_(right_enc), #3 wc );
1493 _dat_( if_c add _lbl_(right_vel), #1 );
1495 _dat_( if_c cmpsub _lbl_(right_count), _lbl_(left_dist) wz );
1497 _dat_( if_c add _lbl_(vel_ctr), _lbl_(right_dom) );
1499 _dat_( if_c add _lbl_(ramp_count), _lbl_(right_dom) );
1501 _dat_( if_c_and_z mov ctra, #0 );
1503 _dat_( test _lbl_(left_enc_bit), ina wc );
1504 _dat_( rcl _lbl_(left_enc), #1 );
1505 _dat_( test _lbl_(left_enc), #3 wc );
1506 _dat_( if_c add _lbl_(left_vel), #1 );
1507 _dat_( if_c cmpsub _lbl_(left_count), _lbl_(right_dist) wz );
1508 _dat_( if_c add _lbl_(vel_ctr), _lbl_(left_dom) );
1509 _dat_( if_c add _lbl_(ramp_count), _lbl_(left_dom) );
1510 _dat_( if_c_and_z mov ctrb, #0 );
1512 _dat_( max _lbl_(ramp_count), _lbl_(max_dist) );
1514 _dat_( mov _lbl_(right_pwm), _lbl_(nominal_pwm) );
1516 _dat_( mov _lbl_(left_pwm), _lbl_(nominal_pwm) );
1518 _dat_( cmp _lbl_(right_count), _lbl_(left_count) wz, wc);
1520 _dat_( if_z jmp #_lbl_(Motor_driver_coord_ok) );
1522 _dat_( if_nc jmp #_lbl_(Motor_driver_lahead) );
1523 _dat_( Motor_driver_rahead: );
1525 _dat_( mov _lbl_(macc), _lbl_(left_count) );
1526 _dat_( sub _lbl_(macc), _lbl_(right_dist) );
1528 _dat_( cmp _lbl_(right_dist), _lbl_(left_dist) wz, wc);
1530 _dat_( if_nc_and_nz cmp _lbl_(right_count), _lbl_(macc) wc );
1532 _dat_( if_c_or_z mov _lbl_(right_pwm), #0 );
1534 _dat_( jmp #_lbl_(Motor_driver_coord_ok) );
1535 _dat_( Motor_driver_lahead: );
1537 _dat_( mov _lbl_(macc), _lbl_(right_count) );
1538 _dat_( sub _lbl_(macc), _lbl_(left_dist) );
1539 _dat_( cmp _lbl_(left_dist), _lbl_(right_dist) wz, wc);
1540 _dat_( if_nc_and_nz cmp _lbl_(left_count), _lbl_(macc) wc );
1541 _dat_( if_c_or_z mov _lbl_(left_pwm), #0 );
1542 _dat_( Motor_driver_coord_ok: );
1544 _dat_( jmpret _lbl_(put_debug_ret), #_lbl_(put_debug) );
1546 _dat_( or _lbl_(right_count), _lbl_(left_count) wz, nr);
1548 _dat_( if_z jmp #_lbl_(Motor_driver_main_lp) );
1550 _dat_(
waitcnt _lbl_(mtime), _lbl_(mdt) );
1552 _dat_( neg phsa, _lbl_(right_pwm) );
1553 _dat_( neg phsb, _lbl_(left_pwm) );
1555 _dat_( djnz _lbl_(motor_ctr), #_lbl_(Motor_driver_motor_lp));
1557 _dat_( mov _lbl_(cur_vel), _lbl_(vel_ctr) );
1559 _dat_( cmp _lbl_(cur_vel), _lbl_(targ_vel) wz, wc );
1560 _dat_( mov _lbl_(macc), _lbl_(cur_vel) );
1561 _dat_( shl _lbl_(macc), #4 );
1562 _dat_( min _lbl_(macc), #$100 );
1564 _dat_( if_nz sumnc _lbl_(nominal_pwm), _lbl_(macc) );
1565 _dat_( mins _lbl_(nominal_pwm), #0 );
1566 _dat_( maxs _lbl_(nominal_pwm), _lbl_(mdt) );
1568 _dat_( djnz _lbl_(stat_ctr), #_lbl_(Motor_driver_motor_lp));
1570 _dat_( rdword _lbl_(maccx), _lbl_(midler_addr) );
1572 _dat_( sub _lbl_(maccx), _lbl_(idler_cnt) wz );
1574 _dat_( if_nz mov _lbl_(idler_timer), #0 );
1576 _dat_( if_z add _lbl_(idler_timer), #1 );
1578 _dat_( if_z max _lbl_(idler_timer), #$ff );
1580 _dat_( add _lbl_(idler_cnt), _lbl_(maccx) );
1582 _dat_( max _lbl_(maccx), #3 );
1584 _dat_( add _lbl_(idler_vel), _lbl_(maccx) );
1586 _dat_( shr _lbl_(maccx), #1 wc );
1587 _dat_( rcr _lbl_(idler_reg), #1 wc );
1589 _dat_( if_c sub _lbl_(idler_vel), #1 );
1590 _dat_( shr _lbl_(maccx), #1 wc );
1591 _dat_( rcr _lbl_(idler_reg), #1 wc );
1592 _dat_( if_c sub _lbl_(idler_vel), #2 );
1594 _dat_( mov _lbl_(mot_stat), #SCRIBBLER_MOT_RUNNING );
1596 _dat_( jmpret _lbl_(put_stat_ret), #_lbl_(put_stat) );
1598 _dat_( jmp #_lbl_(Motor_driver_motor_lp) );
1601 _dat_( jmpret _lbl_(rst_vel_ret), #_lbl_(rst_vel) );
1602 _dat_( mov _lbl_(mot_stat), #SCRIBBLER_MOT_STOPPED );
1606 _dat_( test outa, _lbl_(left_dir_bit) wc );
1608 _dat_( max _lbl_(left_vel), #$7f );
1610 _dat_( negnc _lbl_(maccx), _lbl_(left_vel) );
1612 _dat_( shl _lbl_(maccx), #$18 );
1614 _dat_( or _lbl_(mot_stat), _lbl_(maccx) );
1616 _dat_( test outa, _lbl_(right_dir_bit) wc );
1617 _dat_( max _lbl_(right_vel), #$7f );
1618 _dat_( negnc _lbl_(maccx), _lbl_(right_vel) );
1619 _dat_( shl _lbl_(maccx), #$18 );
1620 _dat_( shr _lbl_(maccx), #8 );
1621 _dat_( or _lbl_(mot_stat), _lbl_(maccx) );
1623 _dat_( mov _lbl_(maccx), _lbl_(idler_timer) );
1624 _dat_( shl _lbl_(maccx), #8 );
1626 _dat_( or _lbl_(mot_stat), _lbl_(maccx) );
1628 _dat_( mov _lbl_(maccx), _lbl_(idler_vel) );
1629 _dat_( shl _lbl_(maccx), #2 );
1631 _dat_( or _lbl_(mot_stat), _lbl_(maccx) );
1633 _dat_( wrlong _lbl_(mot_stat), _lbl_(mstat_addr) );
1634 _dat_( put_stat_ret: );
1635 _dat_( clr_stat_ret: );
1637 _dat_( put_debug: );
1639 _dat_( mov _lbl_(maccx), _lbl_(mstat_addr) );
1640 _dat_( add _lbl_(maccx), #4 );
1641 _dat_( mov _lbl_(macc), _lbl_(max_vel) );
1642 _dat_( shl _lbl_(macc), #8 );
1643 _dat_( or _lbl_(macc), _lbl_(end_vel) );
1644 _dat_( shl _lbl_(macc), #8 );
1645 _dat_( or _lbl_(macc), _lbl_(cur_vel) );
1646 _dat_( shl _lbl_(macc), #8 );
1647 _dat_( or _lbl_(macc), _lbl_(targ_vel) );
1648 _dat_( wrlong _lbl_(macc), _lbl_(maccx) );
1649 _dat_( add _lbl_(maccx), #4 );
1650 _dat_( mov _lbl_(macc), _lbl_(left_dist) );
1651 _dat_( shl _lbl_(macc), #$10 );
1652 _dat_( or _lbl_(macc), _lbl_(right_dist) );
1653 _dat_( wrlong _lbl_(macc), _lbl_(maccx) );
1654 _dat_( add _lbl_(maccx), #4 );
1655 _dat_( mov _lbl_(macc), _lbl_(right_count) );
1656 _dat_( wrlong _lbl_(macc), _lbl_(maccx) );
1657 _dat_( add _lbl_(maccx), #4 );
1658 _dat_( mov _lbl_(macc), _lbl_(left_count) );
1659 _dat_( wrlong _lbl_(macc), _lbl_(maccx) );
1660 _dat_( add _lbl_(maccx), #4 );
1661 _dat_( mov _lbl_(macc), _lbl_(ramp_count) );
1662 _dat_( shl _lbl_(macc), #$10 );
1663 _dat_( or _lbl_(macc), _lbl_(max_dist) );
1664 _dat_( wrlong _lbl_(macc), _lbl_(maccx) );
1665 _dat_( put_debug_ret: );
1670 _dat_( neg _lbl_(idler_reg), #1 );
1672 _dat_( mov _lbl_(idler_vel), #$30 );
1674 _dat_( mov _lbl_(idler_timer), #0 );
1675 _dat_( mov _lbl_(left_vel), #0 );
1676 _dat_( mov _lbl_(right_vel), #0 );
1677 _dat_( mov _lbl_(cur_vel), #0 );
1678 _dat_( mov _lbl_(stat_ctr), #0 );
1679 _dat_( mov _lbl_(motor_ctr), #0 );
1680 _dat_( rst_vel_ret: );
1686 _dat_( shl _lbl_(maccx), #$10 );
1688 _dat_( mov _lbl_(mcnt), #$10 );
1690 _dat_( shr _lbl_(macc), #1 wc );
1691 _dat_( Umult_loop: );
1693 _dat_( if_c add _lbl_(macc), _lbl_(maccx) wc );
1695 _dat_( rcr _lbl_(macc), #1 wc );
1697 _dat_( djnz _lbl_(mcnt), #_lbl_(Umult_loop) );
1698 _dat_( umult_ret: );
1701 _dat_( right_dir_bit: );
1703 _dat_( .
long (1 << SCRIBBLER_MOT_RIGHT_DIR) );
1704 _dat_( left_dir_bit: );
1705 _dat_( .
long (1 << SCRIBBLER_MOT_LEFT_DIR) );
1706 _dat_( both_dir_bits: );
1707 _dat_( .
long ((1 << SCRIBBLER_MOT_RIGHT_DIR) | (1 << SCRIBBLER_MOT_LEFT_DIR)));
1708 _dat_( right_enc_bit: );
1709 _dat_( .
long (1 << SCRIBBLER_MOT_RIGHT_ENC) );
1710 _dat_( left_enc_bit: );
1711 _dat_( .
long (1 << SCRIBBLER_MOT_LEFT_ENC) );
1713 _dat_( .
long ((((1 << SCRIBBLER_MOT_RIGHT_PWM) | (1 << SCRIBBLER_MOT_LEFT_PWM)) | (1 << SCRIBBLER_MOT_RIGHT_DIR)) | (1 << SCRIBBLER_MOT_LEFT_DIR)));
1715 _dat_( .
long ((4 << $1a) | SCRIBBLER_MOT_RIGHT_PWM) );
1717 _dat_( .
long ((4 << $1a) | SCRIBBLER_MOT_LEFT_PWM) );
1719 _dat_( .
long $ffff );
1722 _dat_( .
long $fa0 );
1730 _dat_( idler_vel: );
1732 _dat_( midler_addr: );
1733 _dat_( .
long (0 - 0) );
1736 _dat_( motor_ctr: );
1739 _dat_( mpar_addr: );
1743 _dat_( mstat_addr: );
1758 _dat_( motor_timer: );
1761 _dat_( mdtimeout: );
1764 _dat_( right_enc: );
1770 _dat_( right_dist: );
1773 _dat_( left_dist: );
1779 _dat_( right_count: );
1782 _dat_( left_count: );
1785 _dat_( ramp_count: );
1799 _dat_( right_vel: );
1806 _dat_( idler_cnt: );
1809 _dat_( right_dom: );
1815 _dat_( nominal_pwm: );
1818 _dat_( right_pwm: );
1823 _dat_( idler_reg: );
1825 _dat_( idler_timer: );
1831 _dat_( Adc_sequence: );
1839 _dat_( .word (((SCRIBBLER__VSS | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1840 _dat_( .word (((SCRIBBLER__VDD | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1841 _dat_( .word (((SCRIBBLER__5V | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_5V0));
1842 _dat_( .word (((SCRIBBLER__5V_DIV | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1843 _dat_( .word (((SCRIBBLER__VBAT | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1844 _dat_( .word (((SCRIBBLER__VTRIP | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1845 _dat_( .word (((SCRIBBLER__IDD | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_5V0));
1846 _dat_( .word (((SCRIBBLER__IMOT | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_3V3));
1847 _dat_( .word (((SCRIBBLER__IDLER | SCRIBBLER__SOAK_64US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_3V3));
1848 _dat_( .word (((SCRIBBLER__RIGHT_LGT | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1849 _dat_( .word (((SCRIBBLER__CENTER_LGT | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1850 _dat_( .word (((SCRIBBLER__LEFT_LGT | SCRIBBLER__SOAK_1US) | SCRIBBLER__LPF_64MS) | SCRIBBLER__REF_3V3));
1851 _dat_( .word (((SCRIBBLER__RIGHT_LIN | SCRIBBLER__SOAK_64US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_3V3));
1852 _dat_( .word (((SCRIBBLER__LEFT_LIN | SCRIBBLER__SOAK_64US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_3V3));
1853 _dat_( .word (((SCRIBBLER__P6 | SCRIBBLER__SOAK_16US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_5V0));
1854 _dat_( .word (((SCRIBBLER__P7 | SCRIBBLER__SOAK_16US) | SCRIBBLER__LPF_4MS) | SCRIBBLER__REF_5V0));
1859 _dat_( .
long 0, 0, 0, 0, 0, 0, 0, 0 );
1862 _dat_( Current_x: );
1863 _dat_( .
long (0 - 0) );
1864 _dat_( Current_y: );
1865 _dat_( .
long (0 - 0) );
1866 _dat_( Current_w: );
1867 _dat_( .
long (0 - 0) );
1868 _dat_( Stall_hyst: );
1870 _dat_( Full_circle: );
1871 _dat_( .word SCRIBBLER_DEFAULT_FULL_CIRCLE );
1872 _dat_( Wheel_space: );
1873 _dat_( .word SCRIBBLER_DEFAULT_WHEEL_SPACE );
1874 _dat_( Half_circle: );
1875 _dat_( .word (SCRIBBLER_DEFAULT_FULL_CIRCLE / 2) );
1876 _dat_( Qtr_circle: );
1877 _dat_( .word (SCRIBBLER_DEFAULT_FULL_CIRCLE / 4) );
1878 _dat_( Atan_circle: );
1879 _dat_( .
long ((SCRIBBLER_DEFAULT_FULL_CIRCLE * $de09) / $186a0));
1880 _dat_( Light_scale: );
1881 _dat_( .
byte SCRIBBLER_DEFAULT_LIGHT_SCALE, SCRIBBLER_DEFAULT_LIGHT_SCALE, SCRIBBLER_DEFAULT_LIGHT_SCALE);
1882 _dat_( Line_thld: );
1883 _dat_( .ascii
" " );
1884 _dat_( Obstacle_thld: );
1885 _dat_( .
byte SCRIBBLER_DEFAULT_OBSTACLE_THLD );
1887 _dat_( .
byte 0, 0, 0 );
1889 _dat_( Motor_cmd: );
1892 _dat_( Motor_time: );
1895 _dat_( Motor_Rdist: );
1898 _dat_( Motor_Ldist: );
1901 _dat_( Motor_stat: );
1902 _dat_( .
long 0, 0, 0, 0, 0, 0 );
1903 _dat_( Path_Rdist: );
1905 _dat_( Path_Ldist: );
1907 _dat_( Path_time: );
1909 _dat_( Path_max_spd: );
1913 _dat_( .word 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
1915 _dat_( Tone_queue: );
1916 _dat_( .word 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
1918 _dat_( Tone_enq_ptr: );
1921 _dat_( Tone_deq_ptr: );
1925 _dat_( .ascii
"\0" );
1927 _dat_( Tone_sync: );
1928 _dat_( .ascii
"\0" );
1930 _dat_( Tone_voice1: );
1931 _dat_( .ascii
"\0" );
1933 _dat_( Tone_voice2: );
1934 _dat_( .ascii
"\0" );
1936 _dat_( .ascii
"\0" );
1938 _dat_( .ascii
"\0" );
1940 _dat_( .ascii
"\0" );
1942 _dat_( .ascii
"\0" );
1943 _dat_( Motor_cog: );
1944 _dat_( .ascii
"\0" );
1945 _dat_( Reset_count: );
1946 _dat_( .ascii
"\0" );
1947 _dat_( Current_spd: );
1948 _dat_( .ascii
"\0" );
1949 _dat_( Model_number: );
1950 _dat_( .ascii
"\0" );
1955 _dat_( .equ ..org0001, ..org0001_base );
1956 _dat_( .equ ..org0002, ..org0002_base );
1957 _dat_( .equ ..org0003, ..org0003_base );
1958 _dat_( .equ ..org0004, ..org0004_base );
1959 _dat_( .compress
default );
1968 int32_t scribbler_start(
scribbler *
self)
1970 int32_t i, Register;
1975 scribbler_stop_all();
1979 OUTA = (
OUTA & 0xfc3fffff) | 0x3000000;
1981 DIRA &= (~(3 << 26));
1984 if ((
INA >> SCRIBBLER__MUX_ADC_IN) & 0x1) {
1985 Model_number[0] = SCRIBBLER_MODEL_S2;
1987 Model_number[0] = SCRIBBLER_MODEL_S3;
1989 results_addr[0] = (int32_t)(Results);
1990 memset( (
void *)results_addr[0], 0,
sizeof(uint16_t)*40);
1991 seq_addr[0] = (int32_t)(Adc_sequence);
1992 if ((Adc_cog[0] =
cognew((int32_t)(adc_all), (Model_number[0] << 2)) + 1)) {
1995 scribbler__i2c_stop();
1996 if ((Reset_count[0] = scribbler__ee_rdbyte(SCRIBBLER_EE_RESET_CNT))) {
1997 scribbler__ee_wrbyte(SCRIBBLER_EE_RESET_CNT, 0);
1999 if (Reset_count[0] > 8) {
2002 scribbler_read_wheel_calibration();
2003 scribbler_read_light_calibration();
2004 scribbler_read_line_threshold();
2005 scribbler_set_led(SCRIBBLER_POWER, SCRIBBLER_BLUE);
2006 if (scribbler_get_model_s3()) {
2009 Register = scribbler__BQ2495_rdbyte(0);
2015 scribbler__BQ2495_wrbyte(0, Register);
2017 scribbler__BQ2495_wrbyte(1, 113);
2019 scribbler__BQ2495_wrbyte(2, 92);
2021 scribbler__BQ2495_wrbyte(3, 48);
2023 scribbler__BQ2495_wrbyte(4, 178);
2025 scribbler__BQ2495_wrbyte(5, 142);
2027 scribbler__BQ2495_wrbyte(6, 138);
2029 scribbler__BQ2495_wrbyte(7, 72);
2031 scribbler_delay_tenths(
self, 5);
2038 int32_t scribbler_start_motors(
void)
2044 if (!(Motor_cog[0])) {
2046 midler_addr[0] = (int32_t)(Results) + (SCRIBBLER_CNT_IDLER << 1);
2047 result = -((Motor_cog[0] =
cognew((int32_t)(motor_driver), (int32_t)(Motor_cmd)) + 1) > 0);
2048 while (Motor_cmd[0]) {
2052 scribbler_here_is(0, 0);
2053 scribbler_heading_is(Qtr_circle[0]);
2054 scribbler_set_speed(7);
2059 int32_t scribbler_start_tones(
scribbler *
self)
2065 if (!(Tone_cog[0])) {
2066 memset( (
void *)(int32_t)(Tone_queue), 0,
sizeof(uint16_t)*(SCRIBBLER__TONE_Q_SIZE + 4));
2067 dttime[0] = (
CLKFREQ / 65536) * 2;
2068 queue_addr[0] = (int32_t)(Tone_queue);
2069 result = -((Tone_cog[0] =
cognew((int32_t)(tone_seq), 0) + 1) > 0);
2070 scribbler_command_tone(SCRIBBLER_PLAY);
2075 int32_t scribbler_start_mic_env(
scribbler *
self)
2080 if (!(Mic_cog[0])) {
2082 mic_cyc[0] = 8000 / 20;
2083 return -((Mic_cog[0] =
cognew((int32_t)(env_det), (int32_t)(Envelope)) + 1) > 0);
2088 void scribbler_stop_all(
void)
2090 int32_t _tmp__0002, _tmp__0003, _tmp__0004, _tmp__0005;
2095 __builtin_propeller_cogstop((( ( (_tmp__0002 = Adc_cog[0]), (Adc_cog[0] = 0) ), _tmp__0002 ) - 1));
2098 __builtin_propeller_cogstop((( ( (_tmp__0003 = Tone_cog[0]), (Tone_cog[0] = 0) ), _tmp__0003 ) - 1));
2101 __builtin_propeller_cogstop((( ( (_tmp__0004 = Mic_cog[0]), (Mic_cog[0] = 0) ), _tmp__0004 ) - 1));
2104 __builtin_propeller_cogstop((( ( (_tmp__0005 = Motor_cog[0]), (Motor_cog[0] = 0) ), _tmp__0005 ) - 1));
2113 int32_t scribbler_get_mic_env(
void)
2127 void scribbler_begin_path(
void)
2132 if (!(In_path[0])) {
2137 void scribbler_end_path(
void)
2143 if (In_path[0] == 2) {
2144 scribbler_run_motors(0, Path_Ldist[0], Path_Rdist[0], Path_time[0], Path_max_spd[0], 0);
2149 void scribbler_set_speed(int32_t spd)
2155 Current_spd[0] = spd;
2158 void scribbler_move_to(int32_t
x, int32_t y)
2164 scribbler_move_by((
x - Current_x[0]), (y - Current_y[0]));
2167 void scribbler_arc_to(int32_t
x, int32_t y, int32_t radius)
2177 scribbler_arc_by((
x - Current_x[0]), (y - Current_y[0]), radius);
2180 void scribbler_move_by(int32_t dx, int32_t dy)
2187 scribbler_turn_to((angle = scribbler__atan2(dy, dx)));
2188 scribbler_go_forward((Sqrt__(((dx * dx) + (dy * dy)))));
2189 scribbler_here_is((Current_x[0] + dx), (Current_y[0] + dy));
2192 void scribbler_arc_by(int32_t dx, int32_t dy, int32_t radius)
2194 int32_t dist2, dist, diam, half_angle, tilt;
2202 dist2 = (dx * dx) + (dy * dy);
2203 dist = Sqrt__(dist2);
2204 tilt = scribbler__atan2(dy, dx);
2205 diam = (abs(radius)) << 1;
2207 scribbler_turn_to(tilt);
2208 scribbler_go_forward((dist - diam));
2210 dist2 = dist * dist;
2212 half_angle = scribbler__atan2((Shr__(dist, 1)), (Sqrt__(((radius * radius) - (Shr__(dist2, 2))))));
2214 half_angle = -half_angle;
2216 scribbler_turn_to((tilt - half_angle));
2217 scribbler_arc((half_angle << 1), radius);
2218 scribbler_here_is((Current_x[0] + dx), (Current_y[0] + dy));
2219 scribbler_heading_is((tilt + half_angle));
2222 int32_t scribbler_align_with(int32_t heading)
2232 dw = (heading - Current_w[0]) % Full_circle[0];
2233 dw = dw + (Full_circle[0] & -(dw < 0));
2234 if (dw > Half_circle[0]) {
2235 dw = dw - Full_circle[0];
2237 if ((abs(dw)) <= Qtr_circle[0]) {
2240 dw = (dw - Half_circle[0]) + (Full_circle[0] & -(dw < 0));
2244 scribbler_heading_is((Current_w[0] + dw));
2248 void scribbler_turn_to_deg(int32_t heading)
2253 scribbler_turn_to(((heading * Full_circle[0]) / 360));
2256 void scribbler_turn_to(int32_t heading)
2261 scribbler_turn_by((heading - Current_w[0]));
2264 void scribbler_turn_by_deg(int32_t dw)
2271 scribbler_turn_by(((dw * Full_circle[0]) / 360));
2274 int32_t scribbler_turn_by(int32_t dw)
2281 dw = dw % Full_circle[0];
2282 dw = dw + (Full_circle[0] & -(dw < 0));
2283 if (dw > Half_circle[0]) {
2284 dw = dw - Full_circle[0];
2287 scribbler_heading_is((Current_w[0] + dw));
2291 void scribbler_here_is(int32_t
x, int32_t y)
2300 void scribbler_heading_is_deg(int32_t w)
2305 scribbler_heading_is(((w * Full_circle[0]) / 360));
2308 void scribbler_heading_is(int32_t w)
2313 Current_w[0] = w % Full_circle[0];
2314 Current_w[0] = Current_w[0] + (Full_circle[0] & -(Current_w[0] < 0));
2325 int32_t scribbler_read_wheel_calibration(
void)
2327 int32_t _local__0006[2];
2333 if (scribbler__ee_rdblock((int32_t)(&_local__0006[0]), SCRIBBLER_EE_WHEEL_CALIB, 4)) {
2334 _local__0006[1] = Shr__(_local__0006[0], 16);
2335 _local__0006[0] &= 0xffff;
2336 if ((((_local__0006[0] > 900) && (_local__0006[0] < 1000)) && (_local__0006[1] > 100)) && (_local__0006[1] < 200)) {
2337 return scribbler_set_wheel_calibration(_local__0006[0], _local__0006[1]);
2340 return scribbler_default_wheel_calibration();
2343 int32_t scribbler_write_wheel_calibration(
void)
2349 return scribbler__ee_wrblock((int32_t)(Full_circle), SCRIBBLER_EE_WHEEL_CALIB, 4);
2352 int32_t scribbler_default_wheel_calibration(
void)
2359 return scribbler_set_wheel_calibration(SCRIBBLER_DEFAULT_FULL_CIRCLE, SCRIBBLER_DEFAULT_WHEEL_SPACE);
2362 int32_t scribbler_set_wheel_calibration(int32_t
circle, int32_t space)
2370 Wheel_space[0] = space;
2371 scribbler__compute_calibration();
2372 return scribbler_get_wheel_calibration();
2375 int32_t scribbler_get_wheel_calibration(
void)
2379 return ((Full_circle[0] << 16) | Wheel_space[0]);
2382 void scribbler_go_left(int32_t dist)
2388 scribbler_turn_deg(90);
2389 scribbler_go_forward(dist);
2392 void scribbler_go_right(int32_t dist)
2398 scribbler_turn_deg((-90));
2399 scribbler_go_forward(dist);
2402 void scribbler_go_forward(int32_t dist)
2407 if ((abs(dist)) == SCRIBBLER_FOREVER) {
2408 scribbler_move(100, 100, 0, Current_spd[0], 1);
2410 scribbler_move(dist, dist, 0, Current_spd[0], 0);
2414 void scribbler_go_back(int32_t dist)
2419 if ((abs(dist)) == SCRIBBLER_FOREVER) {
2420 scribbler_move((-100), (-100), 0, Current_spd[0], 1);
2422 scribbler_move((-dist), (-dist), 0, Current_spd[0], 0);
2426 void scribbler_turn_deg(int32_t ccw_degrees)
2432 scribbler_arc_deg(ccw_degrees, 0);
2435 void scribbler_arc_deg(int32_t ccw_degrees, int32_t radius)
2443 scribbler_arc(((Full_circle[0] * ccw_degrees) / 360), radius);
2446 void scribbler_turn(int32_t ccw_units)
2452 scribbler_arc(ccw_units, 0);
2455 void scribbler_arc(int32_t ccw_units, int32_t radius)
2464 r = (ccw_units * (radius + Wheel_space[0])) / Wheel_space[0];
2465 l = (ccw_units * (radius - Wheel_space[0])) / Wheel_space[0];
2466 scribbler_move(l, r, 0, Current_spd[0], 0);
2469 void scribbler_turn_deg_now(int32_t ccw_degrees)
2476 scribbler_arc_deg_now(ccw_degrees, 0);
2479 void scribbler_arc_deg_now(int32_t ccw_degrees, int32_t radius)
2489 scribbler_arc_now(((Full_circle[0] * ccw_degrees) / 360), radius);
2492 void scribbler_turn_now(int32_t ccw_units)
2499 scribbler_arc_now(ccw_units, 0);
2502 void scribbler_arc_now(int32_t ccw_units, int32_t radius)
2512 r = (ccw_units * (radius + Wheel_space[0])) / Wheel_space[0];
2513 l = (ccw_units * (radius - Wheel_space[0])) / Wheel_space[0];
2514 scribbler_move_now(l, r, 0, Current_spd[0], 0);
2517 int32_t scribbler_move(int32_t left_distance, int32_t right_distance, int32_t move_time, int32_t max_speed, int32_t no_stop)
2519 int32_t max_d, max_pd, max_rvel, max_lvel, end_spd;
2533 left_distance = Min__((Max__((-32767), left_distance)), 32767);
2534 right_distance = Min__((Max__((-32767), right_distance)), 32767);
2535 max_speed = Min__((Max__(0, max_speed)), 15);
2536 Current_spd[0] = max_speed;
2537 if (In_path[0] == 2) {
2538 if (((left_distance ^ Path_Ldist[0]) & 0x8000) || ((right_distance ^ Path_Rdist[0]) & 0x8000)) {
2541 max_d = Max__((abs(left_distance)), (abs(right_distance)));
2542 max_pd = Max__((abs(Path_Ldist[0])), (abs(Path_Rdist[0])));
2546 max_rvel = ((Min__(((((abs(right_distance)) * max_speed) + (Shr__(max_d, 1))) / max_d), ((((abs(Path_Rdist[0])) * Path_max_spd[0]) + (Shr__(max_pd, 1))) / max_pd))) * max_d) / (Max__((abs(right_distance)), 1));
2550 max_lvel = ((Min__(((((abs(left_distance)) * max_speed) + (Shr__(max_d, 1))) / max_d), ((((abs(Path_Ldist[0])) * Path_max_spd[0]) + (Shr__(max_pd, 1))) / max_pd))) * max_d) / (Max__((abs(left_distance)), 1));
2551 end_spd = Min__(max_rvel, max_lvel);
2553 scribbler_run_motors(0, Path_Ldist[0], Path_Rdist[0], Path_time[0], Path_max_spd[0], end_spd);
2556 if (In_path[0] >= 1) {
2557 if ((no_stop) && (move_time == 0)) {
2558 scribbler_run_motors(SCRIBBLER_MOT_CONT, left_distance, right_distance, move_time, max_speed, 0);
2561 Path_Ldist[0] = left_distance;
2562 Path_Rdist[0] = right_distance;
2563 Path_time[0] = move_time;
2564 Path_max_spd[0] = max_speed;
2568 scribbler_run_motors((SCRIBBLER_MOT_CONT & -(no_stop != 0)), left_distance, right_distance, move_time, max_speed, 0);
2573 void scribbler_wheels_now(int32_t left_velocity, int32_t right_velocity, int32_t move_time)
2581 scribbler_move_now(left_velocity, right_velocity, move_time, (Shr__((Min__((Max__((abs(left_velocity)), (abs(right_velocity)))), 255)), 4)), 1);
2584 void scribbler_move_now(int32_t left_distance, int32_t right_distance, int32_t move_time, int32_t max_speed, int32_t no_stop)
2598 scribbler_run_motors((SCRIBBLER_MOT_IMM | (SCRIBBLER_MOT_CONT & -(no_stop != 0))), left_distance, right_distance, move_time, max_speed, 0);
2601 void scribbler_stop_now(
void)
2607 scribbler_run_motors(SCRIBBLER_MOT_IMM, 0, 0, 0, 0, 0);
2610 void scribbler_wait_stop(
void)
2615 while (scribbler_moving()) {
2620 int32_t scribbler_stalled(
void)
2622 int32_t
stat, mvel, ivel, itime, vstall, istall;
2628 if ((
stat = Motor_stat[0]) & 0x3) {
2629 mvel = abs(((
stat >> 24) + ((
stat << 8) >> 24)));
2630 itime = (Shr__(
stat, 8)) & 0xff;
2631 if (!((ivel = (
stat & 0xfc) << 3))) {
2634 if ((vstall = -(((mvel * 14) / ivel) > (20 + Stall_hyst[0])))) {
2639 istall = -(scribbler_get_adc_results(SCRIBBLER_ADC_IMOT) > (Shr__((75 * scribbler_get_adc_results(SCRIBBLER_ADC_VBAT)), 7)));
2640 return -(vstall || istall);
2645 int32_t scribbler_moving(
void)
2650 return -((Motor_stat[0] & 0x3) != 0);
2653 int32_t scribbler_motion(
void)
2670 return Motor_stat[0];
2673 int32_t scribbler_motion_addr(
void)
2678 return (int32_t)(Motor_stat);
2681 int32_t scribbler_move_ready(
void)
2686 return -(Motor_cmd[0] == 0);
2689 void scribbler_run_motors(int32_t command, int32_t left_distance, int32_t right_distance, int32_t
timeout, int32_t max_speed, int32_t end_speed)
2717 if (command & SCRIBBLER_MOT_IMM) {
2718 ((int32_t *)(int32_t)(Motor_Rdist))[0] = 0;
2719 ((int32_t *)(int32_t)(Motor_cmd))[0] = 0;
2721 while (((int32_t *)(int32_t)(Motor_cmd))[0]) {
2725 Motor_Rdist[0] = Min__((Max__((-32767), right_distance)), 32767);
2726 Motor_Ldist[0] = Min__((Max__((-32767), left_distance)), 32767);
2727 ((int32_t *)(int32_t)(Motor_cmd))[0] = (((
timeout << 16) | ((Min__((Max__(0, max_speed)), 15)) << 8)) | ((Min__((Max__(0, end_speed)), 15)) << 4)) | (command & (SCRIBBLER_MOT_IMM | SCRIBBLER_MOT_CONT));
2728 if ((left_distance) || (right_distance)) {
2730 while (!((scribbler_moving()) || ((
CNT -
timeout) > 800000))) {
2745 int32_t scribbler_read_obstacle_threshold(
void)
2747 int32_t _local__0007[1];
2759 if (scribbler__ee_rdblock((int32_t)(&_local__0007[0]), SCRIBBLER_EE_OBSTACLE_THLD, 1)) {
2760 _local__0007[0] &= 0xff;
2761 if ((_local__0007[0] > 0) && (_local__0007[0] <= 100)) {
2762 return scribbler_set_obstacle_threshold(_local__0007[0]);
2765 return scribbler_default_obstacle_threshold();
2768 int32_t scribbler_write_obstacle_threshold(
void)
2778 return scribbler__ee_wrblock((int32_t)(Obstacle_thld), SCRIBBLER_EE_OBSTACLE_THLD, 1);
2781 int32_t scribbler_default_obstacle_threshold(
void)
2792 return scribbler_set_obstacle_threshold(SCRIBBLER_DEFAULT_OBSTACLE_THLD);
2795 int32_t scribbler_set_obstacle_threshold(int32_t thld)
2806 return (Obstacle_thld[0] = thld);
2809 int32_t scribbler_get_obstacle_threshold(
void)
2816 return Obstacle_thld[0];
2819 int32_t scribbler_obstacle(
scribbler *
self, int32_t side, int32_t threshold)
2834 threshold = Obstacle_thld[0];
2836 threshold = Min__((Max__(threshold, 1)), 100);
2837 FRQA = (14000 * threshold) + (20607 * (100 - threshold));
2838 if (side == SCRIBBLER_LEFT) {
2839 CTRA = (4 << 26) | SCRIBBLER_OBS_TX_LEFT;
2840 DIRA |= (1 << SCRIBBLER_OBS_TX_LEFT);
2842 if (side == SCRIBBLER_RIGHT) {
2843 CTRA = (4 << 26) | SCRIBBLER_OBS_TX_RIGHT;
2844 DIRA |= (1 << SCRIBBLER_OBS_TX_RIGHT);
2847 _waitcnt((
CNT + 24000));
2848 result = -(((
INA >> SCRIBBLER_OBS_RX) & 0x1) == 0);
2849 DIRA &= (~(1 << SCRIBBLER_OBS_TX_LEFT));
2850 DIRA &= (~(1 << SCRIBBLER_OBS_TX_RIGHT));
2859 int32_t scribbler_read_line_threshold(
void)
2861 int32_t _local__0008[1];
2869 if (scribbler__ee_rdblock((int32_t)(&_local__0008[0]), SCRIBBLER_EE_LINE_THLD, 1)) {
2870 _local__0008[0] &= 0xff;
2871 if ((_local__0008[0] > 5) && (_local__0008[0] < 100)) {
2872 return scribbler_set_line_threshold(_local__0008[0]);
2875 return scribbler_default_line_threshold();
2878 int32_t scribbler_write_line_threshold(
void)
2884 return scribbler__ee_wrblock((int32_t)(Line_thld), SCRIBBLER_EE_LINE_THLD, 1);
2887 int32_t scribbler_default_line_threshold(
void)
2895 return scribbler_set_line_threshold(SCRIBBLER_DEFAULT_LINE_THLD);
2898 int32_t scribbler_set_line_threshold(int32_t thld)
2906 return (Line_thld[0] = thld);
2909 int32_t scribbler_get_line_threshold(
void)
2914 return Line_thld[0];
2917 int32_t scribbler_line_sensor(int32_t side, int32_t threshold)
2928 if (side == SCRIBBLER_LEFT) {
2929 result = Shr__(((uint16_t *)results_addr[0])[SCRIBBLER_ADC_LEFT_LIN], 8);
2931 if (side == SCRIBBLER_RIGHT) {
2932 result = Shr__(((uint16_t *)results_addr[0])[SCRIBBLER_ADC_RIGHT_LIN], 8);
2935 if (threshold == 0) {
2936 result = -(result >= Line_thld[0]);
2938 if (threshold > 0) {
2939 result = -(result >= threshold);
2945 int32_t scribbler_read_light_calibration(
void)
2947 int32_t _local__0009[1];
2954 if (scribbler__ee_rdblock((int32_t)(&_local__0009[0]), SCRIBBLER_EE_LIGHT_CALIB, 3)) {
2955 return scribbler_set_light_calibration(((_local__0009[0] << 24) >> 24), ((_local__0009[0] << 16) >> 24), ((_local__0009[0] << 8) >> 24));
2957 return scribbler_default_light_calibration();
2961 int32_t scribbler_write_light_calibration(
void)
2963 int32_t _local__0010[1];
2968 _local__0010[0] = scribbler_get_light_calibration();
2969 return scribbler__ee_wrblock((int32_t)(&_local__0010[0]), SCRIBBLER_EE_LIGHT_CALIB, 3);
2972 int32_t scribbler_default_light_calibration(
void)
2980 return scribbler_set_light_calibration(SCRIBBLER_DEFAULT_LIGHT_SCALE, SCRIBBLER_DEFAULT_LIGHT_SCALE, SCRIBBLER_DEFAULT_LIGHT_SCALE);
2983 int32_t scribbler_set_light_calibration(int32_t left_scale, int32_t center_scale, int32_t right_scale)
2985 int32_t _parm__0011[4];
2986 _parm__0011[0] = left_scale;
2987 _parm__0011[1] = center_scale;
2988 _parm__0011[2] = right_scale;
2997 for(_parm__0011[3] = 0; _parm__0011[3] <= 2; _parm__0011[3]++) {
2998 Light_scale[_parm__0011[3]] = Min__((Max__((&_parm__0011[0])[_parm__0011[3]], (-128))), 127);
3000 return scribbler_get_light_calibration();
3003 int32_t scribbler_get_light_calibration(
void)
3013 return ((Light_scale[0] | (Light_scale[1] << 8)) | (Light_scale[2] << 16));
3016 int32_t scribbler_light_sensor(int32_t side)
3024 if ((result = scribbler_light_sensor_word(side)) == SCRIBBLER_UNDEF) {
3027 result = Sqrt__(result);
3031 int32_t scribbler_light_sensor_log(int32_t side)
3033 int32_t wsense, lsense, ssense, mant, Char;
3035 static int32_t look__0001[] = {SCRIBBLER_LEFT, SCRIBBLER_CENTER, SCRIBBLER_RIGHT, };
3042 if ((wsense = scribbler_light_sensor_word(side)) == SCRIBBLER_UNDEF) {
3045 if ((Char = (BitEncode__(wsense)) - 1) < 0) {
3048 lsense = Shr__((wsense << (31 - Char)), 20);
3049 mant = ((uint16_t *)(49152 + ((lsense & 0x7ff) << 1)))[0];
3050 lsense = Min__(((((Char << 4) | (Shr__(mant, 12))) << 8) / 245), 255);
3051 ssense = Sqrt__((Min__(((wsense << 15) / 30000), 65535)));
3052 result = Min__((Max__(((((ssense * (255 - ssense)) + (lsense * ssense)) / 255) + ((Light_scale[Lookdown__(side, 0, look__0001, 3)] << 24) >> 24)), 0)), 255);
3056 int32_t scribbler_light_sensor_raw(int32_t side)
3062 if ((result = scribbler_light_sensor_word(side)) == SCRIBBLER_UNDEF) {
3065 result = Shr__(result, 4);
3069 int32_t scribbler_light_sensor_word(int32_t side)
3071 if (side == SCRIBBLER_LEFT) {
3072 return ((uint16_t *)results_addr[0])[SCRIBBLER_ADC_LEFT_LGT];
3074 if (side == SCRIBBLER_CENTER) {
3075 return ((uint16_t *)results_addr[0])[SCRIBBLER_ADC_CENTER_LGT];
3077 if (side == SCRIBBLER_RIGHT) {
3078 return ((uint16_t *)results_addr[0])[SCRIBBLER_ADC_RIGHT_LGT];
3080 return SCRIBBLER_UNDEF;
3086 int32_t scribbler_get_results(int32_t index)
3091 return ((uint16_t *)results_addr[0])[index];
3094 int32_t scribbler_get_adc_results(int32_t index)
3099 return (Shr__(((uint16_t *)results_addr[0])[index], 8));
3107 int32_t scribbler_button_press(
void)
3112 return -(((
INA >> SCRIBBLER_BUTTON) & 0x1) == 0);
3115 int32_t scribbler_button_count(
void)
3121 if ((result = ((uint8_t *)results_addr[0])[0x2c])) {
3122 ((uint8_t *)results_addr[0])[0x2c] = 0;
3127 int32_t scribbler_reset_button_count(
void)
3132 return Reset_count[0];
3135 void scribbler_button_mode(int32_t led_enable, int32_t reset_enable)
3137 ((uint8_t *)results_addr[0])[0x2d] = (SCRIBBLER_LED_ENA & -(led_enable != 0)) | (SCRIBBLER_RST_ENA & -(reset_enable != 0));
3145 void scribbler_set_leds(int32_t left_color, int32_t center_color, int32_t right_color, int32_t power_color)
3151 if (left_color != SCRIBBLER_NO_CHANGE) {
3152 scribbler_set_led(SCRIBBLER_LEFT, left_color);
3154 if (center_color != SCRIBBLER_NO_CHANGE) {
3155 scribbler_set_led(SCRIBBLER_CENTER, center_color);
3157 if (right_color != SCRIBBLER_NO_CHANGE) {
3158 scribbler_set_led(SCRIBBLER_RIGHT, right_color);
3160 if (power_color != SCRIBBLER_NO_CHANGE) {
3161 scribbler_set_led(SCRIBBLER_POWER, power_color);
3165 void scribbler_set_led(int32_t index, int32_t color)
3168 ((uint8_t *)((results_addr[0] + (SCRIBBLER_LED_BYTES << 1)) + (index & 0x3)))[0] = color;
3176 int32_t scribbler_beep(
void)
3181 scribbler_set_volume(50);
3182 scribbler_set_voices(SCRIBBLER_SIN, SCRIBBLER_SIN);
3183 scribbler_play_tone(150, 1000, 0);
3184 return scribbler_play_tone(350, 0, 0);
3187 void scribbler_command_tone(int32_t cmd_tone)
3197 case SCRIBBLER_STOP:
3198 case SCRIBBLER_PAUSE:
3199 case SCRIBBLER_PLAY:
3200 Tone_cmd[0] = cmd_tone;
3201 while (Tone_cmd[0]) {
3204 if (cmd_tone == SCRIBBLER_STOP) {
3205 Tone_enq_ptr[0] = Tone_deq_ptr[0];
3211 int32_t scribbler_set_volume(int32_t vol)
3216 return scribbler__enqueue_tone((0x2000 | (((Min__((Max__(vol, 0)), 100)) * 8191) / 100)));
3219 void scribbler_set_voices(int32_t v1, int32_t v2)
3224 Tone_voice1[0] = v1 & 0x3;
3225 Tone_voice2[0] = v2 & 0x3;
3228 int32_t scribbler_play_sync(int32_t value)
3238 return scribbler__enqueue_tone((0x4000 | (value & 0xff)));
3241 int32_t scribbler_play_pause(int32_t value)
3250 return scribbler__enqueue_tone((0x6000 | (value & 0xff)));
3253 int32_t scribbler_get_sync(
void)
3260 if ((result = Tone_sync[0])) {
3266 int32_t scribbler_play_tone(int32_t time, int32_t frq1, int32_t frq2)
3275 scribbler__enqueue_tone((time & 0x1fff));
3276 scribbler__enqueue_tone(((Tone_voice1[0] << 14) | (frq1 & 0x3fff)));
3277 return scribbler__enqueue_tone(((Tone_voice2[0] << 14) | (frq2 & 0x3fff)));
3280 int32_t scribbler_play_tones(int32_t addr)
3323 while (((uint16_t *)addr)[0]) {
3324 result = scribbler__enqueue_tone(((uint16_t *)addr)[0]);
3330 void scribbler_wait_sync(int32_t value)
3337 while (!(Tone_sync[0] == value)) {
3341 while (!(Tone_deq_ptr[0] == Tone_enq_ptr[0])) {
3354 void scribbler_start_timer(int32_t number)
3360 if ((number >= 0) && (number < 8)) {
3361 Timers[number] = ((int32_t *)(results_addr[0] + 0xa))[0];
3365 int32_t scribbler_get_timer(int32_t number)
3371 return (((int32_t *)(results_addr[0] + 0x28))[0] - (Timers[number] & -(-(number >= 0) && -(number < 8))));
3374 void scribbler_delay_tenths(
scribbler *
self, int32_t time)
3376 int32_t time0, _idx__0012, _limit__0013;
3381 for(( (_idx__0012 = 0), (_limit__0013 = time) ); _idx__0012 < _limit__0013; _idx__0012++) {
3382 _waitcnt((time0 = time0 + (
CLKFREQ / 10)));
3391 int32_t scribbler_ee_read_byte(int32_t addr)
3396 return scribbler__ee_rdbyte(addr);
3399 int32_t scribbler_ee_write_byte(int32_t addr, int32_t data)
3404 if (addr >= SCRIBBLER_EE_USER_AREA) {
3405 scribbler__ee_wrbyte(addr, data);
3416 int32_t scribbler_get_model_s2(
void)
3421 return -(Model_number[0] == SCRIBBLER_MODEL_S2);
3424 int32_t scribbler_get_model_s3(
void)
3429 return -(Model_number[0] == SCRIBBLER_MODEL_S3);
3436 int32_t scribbler_set_power_off(
void)
3440 if (scribbler_get_model_s3()) {
3442 scribbler__BQ2495_wrbyte(5, 142);
3444 return scribbler__BQ2495_wrbyte(7, 104);
3450 int32_t scribbler_get_usb_powered(
void)
3455 if (scribbler_get_model_s3()) {
3456 return (scribbler__BQ2495_rdbyte(8) & (1 << 2));
3462 int32_t scribbler_get_charging(
void)
3468 if (scribbler_get_model_s3()) {
3469 Register = scribbler__BQ2495_rdbyte(8) & 0x30;
3470 return -(-(Register == 32) || -(Register == 16));
3483 static void scribbler__compute_calibration(
void)
3486 Half_circle[0] = Shr__(Full_circle[0], 1);
3487 Qtr_circle[0] = Shr__(Full_circle[0], 2);
3488 Atan_circle[0] = (Full_circle[0] * 56841) / 100000;
3491 static int32_t scribbler__atan2(int32_t y, int32_t
x)
3493 int32_t arg, adder, n;
3497 if ((n = BitEncode__((Min__((abs(
x)), (abs(y)))))) > 21) {
3501 if ((abs(
x)) > (abs(y))) {
3502 arg = (y << 10) /
x;
3503 adder = Half_circle[0] & -(
x < 0);
3505 arg = ((-
x) << 10) / y;
3506 adder = Qtr_circle[0] + (Half_circle[0] & -(y < 0));
3508 result = (Shr__(((((abs(arg)) * Atan_circle[0]) / (914 + (Shr__((arg * arg), 12)))) + 1), 2)) - -((abs(arg)) >= 960);
3512 result = result + adder;
3513 result = result + (Full_circle[0] & -(result < 0));
3517 static int32_t scribbler__enqueue_tone(int32_t tone_word)
3523 next_ptr = Tone_enq_ptr[0] + 1;
3524 next_ptr &= -(next_ptr < SCRIBBLER__TONE_Q_SIZE);
3525 while (!(Tone_deq_ptr[0] != next_ptr)) {
3528 Tone_queue[Tone_enq_ptr[0]] = tone_word;
3529 Tone_enq_ptr[0] = next_ptr;
3530 return ((Tone_enq_ptr[0] << 16) | Tone_deq_ptr[0]);
3536 static int32_t scribbler__i2c_rd(int32_t acknak)
3538 int32_t _idx__0014, _mask_0015;
3540 _mask_0015 = 1 << SCRIBBLER_SDA;
3541 for(_idx__0014 = 0; _idx__0014 < 8; _idx__0014++) {
3542 OUTA |= (1 << SCRIBBLER_SCL);
3543 result = (result << 1) | ((
INA >> SCRIBBLER_SDA) & 0x1);
3544 OUTA &= (~(1 << SCRIBBLER_SCL));
3546 if (-(acknak != 0) & 0x1) {
3549 DIRA &= (~_mask_0015);
3551 OUTA |= (1 << SCRIBBLER_SCL);
3552 OUTA &= (~(1 << SCRIBBLER_SCL));
3553 DIRA &= (~(1 << SCRIBBLER_SDA));
3557 static int32_t scribbler__i2c_wr(int32_t data)
3559 int32_t _mask_0016, _idx__0017;
3561 _mask_0016 = 1 << SCRIBBLER_SDA;
3562 for(_idx__0017 = 0; _idx__0017 < 8; _idx__0017++) {
3563 if (-(((data = data << 1) & 0x100) == 0) & 0x1) {
3566 DIRA &= (~_mask_0016);
3568 OUTA |= (1 << SCRIBBLER_SCL);
3569 OUTA &= (~(1 << SCRIBBLER_SCL));
3571 DIRA &= (~(1 << SCRIBBLER_SDA));
3572 OUTA |= (1 << SCRIBBLER_SCL);
3573 result = -(((
INA >> SCRIBBLER_SDA) & 0x1) == 0);
3574 OUTA &= (~(1 << SCRIBBLER_SCL));
3578 static void scribbler__i2c_start(
void)
3580 DIRA &= (~(1 << SCRIBBLER_SDA));
3581 OUTA |= (1 << SCRIBBLER_SCL);
3582 DIRA |= (1 << SCRIBBLER_SDA);
3583 OUTA &= (~(1 << SCRIBBLER_SCL));
3586 static void scribbler__i2c_stop(
void)
3588 OUTA &= (~(1 << SCRIBBLER_SCL));
3589 DIRA |= (1 << SCRIBBLER_SDA);
3590 OUTA |= (1 << SCRIBBLER_SCL);
3591 DIRA &= (~(1 << SCRIBBLER_SDA));
3595 static int32_t scribbler__ee_rdblock(int32_t dest_addr, int32_t addr, int32_t size)
3597 int32_t i, csum, _limit__0018, _step__0019;
3598 csum = scribbler__ee_rdbyte((addr + size));
3599 for(( ( ( (i = 0), (_limit__0018 = size - 1) ), (_step__0019 = 1) ), (_step__0019 = ((i >= _limit__0018) ? -_step__0019 : _step__0019)) ); (((0 <= i) && (i <= _limit__0018)) || ((0 >= i) && (i >= _limit__0018))) || (i == 0); i = i + _step__0019) {
3600 csum = csum + (((uint8_t *)dest_addr)[i] = scribbler__ee_rdbyte((addr + i)));
3602 return -((csum & 0xff) == 0);
3605 static int32_t scribbler__ee_wrblock(int32_t src_addr, int32_t addr, int32_t size)
3607 int32_t i, data, csum, _limit__0020, _step__0021;
3611 for(( ( ( (i = 0), (_limit__0020 = size - 1) ), (_step__0021 = 1) ), (_step__0021 = ((i >= _limit__0020) ? -_step__0021 : _step__0021)) ); (((0 <= i) && (i <= _limit__0020)) || ((0 >= i) && (i >= _limit__0020))) || (i == 0); i = i + _step__0021) {
3612 data = ((uint8_t *)src_addr)[i];
3614 result &= scribbler__ee_wrbyte((addr + i), data);
3616 result &= scribbler__ee_wrbyte((addr + size), (csum & 0xff));
3620 static int32_t scribbler__ee_rdbyte(int32_t addr)
3623 scribbler__ee_waddr(addr);
3624 scribbler__i2c_start();
3625 if (scribbler_get_model_s2()) {
3626 scribbler__i2c_wr(163);
3628 scribbler__i2c_wr(161);
3629 addr = addr + 32768;
3631 result = scribbler__i2c_rd(SCRIBBLER_NAK);
3632 scribbler__i2c_stop();
3636 static int32_t scribbler__ee_wrbyte(int32_t addr, int32_t data)
3639 scribbler__ee_waddr(addr);
3640 result = scribbler__i2c_wr(data);
3641 scribbler__i2c_stop();
3645 static void scribbler__ee_waddr(int32_t addr)
3647 if (scribbler_get_model_s2()) {
3649 scribbler__i2c_start();
3650 }
while (!(scribbler__i2c_wr(162)));
3653 scribbler__i2c_start();
3654 }
while (!(scribbler__i2c_wr(160)));
3655 addr = addr + 32768;
3657 scribbler__i2c_wr((Shr__(addr, 8)));
3658 scribbler__i2c_wr((addr & 0xff));
3662 static int32_t scribbler__BQ2495_rdbyte(int32_t addr)
3665 scribbler__BQ2495_waddr(addr);
3666 scribbler__i2c_start();
3667 scribbler__i2c_wr(215);
3668 result = scribbler__i2c_rd(SCRIBBLER_NAK);
3669 scribbler__i2c_stop();
3673 static int32_t scribbler__BQ2495_wrbyte(int32_t addr, int32_t data)
3676 scribbler__BQ2495_waddr(addr);
3677 result = scribbler__i2c_wr(data);
3678 scribbler__i2c_stop();
3682 static void scribbler__BQ2495_waddr(int32_t addr)
3685 scribbler__i2c_start();
3686 }
while (!(scribbler__i2c_wr(214)));
3687 scribbler__i2c_wr((Min__(addr, 10)));