-
Notifications
You must be signed in to change notification settings - Fork 3
/
scribbler.spin
3013 lines (2409 loc) · 251 KB
/
scribbler.spin
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
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
'':::::::[ Driver Object for the Scribbler Robot Models S2 & S3 ]:::::::::::::::::::::::::::::::::
{{{
┌───────────────────────────────────────┐
│ Scribbler Object │
│(c) Copyright 2010 Bueno Systems, Inc. │
│ See end of file for terms of use. │
└───────────────────────────────────────┘
This object provides both high- and low-level drivers for the
Parallax Scribbler S2 & S3 robots.
Version History
───────────────
2010.09.16: Initial Version 1(M) release
2010.09.28: Version 1(N) release:
Fixed bug in _ee* Spin routines.
Upped size of sound buffer to 1200.
Added calibration methods for light sensors.
Added 1/2 sec. delay to start routine.
2010.10.18: Added check for zero time in play_tone.
2010.10.25: Added log response method for light sensors.
2010.10.27: Version 1(O) release:
Added methods to save default line sensor threshold.
2010.10.28: Fixed bugs in calibration methods.
2010.11.15: Version 1(P) release:
Added self-documentation features.
Added sensitivity setting for obstacle detection.
2015.03.11: Ben Wirz - modified the original S2 object to suport both the S2 & S3
Renamed to the scribbler object from the S2 object.
Added support for detecting the hardware model number.
Added support for the S3's different EEPROM type and layout.
Added basic support for controlling the battery charger IC.
}}
{{=======[ Introduction ]=========================================================
scribbler.spin provides low-level drivers for the Scribblers's various functions, as
well as top-level access functions to interface with those drivers. Driver functions
are separated into four additional cogs:
''
'' 1. Analog, button, and LED drivers.
'' 2. Motor driver.
'' 3. Sound sequencer and synthesizer.
'' 4. Microphone envelope detector.
''
Driver #1 is required and is started with the scribbler object's main `start method.
The other drivers are optional, depending upon the user's requirements, and have their
own start methods (`start_motors, `start_tones, `start_mic_env). The scribbler object's
`stop method stops all driver cogs which have been started.
The analog, button, and LED drivers are the heartbeat of the system. The analog
driver continuously cycles through sixteen analog inputs and updates their states
with a one-millisecond cycle time. The button driver monitors the state
of the scribblers's single pushbutton and, depending on the user-selected button mode, can
cause it to reset the scribbler, while displaying the button-press count on the LEDs, and
record the number of button presses in EEPROM for use by the newly-restarted user
program. The LED driver manages the LEDs' polarity and PWMing to provide various
shades and hues from the scribbler's red/green LEDs and blue power LED. It doesn this via
the LEDs' shift register port.
}}
''=======[ Constants... ]=========================================================
CON
''-[ Version, etc. ]-
{{ Version numbers and miscellaneous other constants. }}
VERSION = 2 'Major version ID.
SUBVERSION = 2578 'Minor version ID.
_TONE_Q_SIZE = 1200 'Sets the size of the sound buffer (words).
NO_CHANGE = -1
FOREVER = $7fff_ffff
UNDEF = $8000_0000
ACK = true
NAK = false
#0, NONE, LEFT, RIGHT, CENTER, POWER 'Used with LEDs, light sensors, line sensors,
'obstacle sensors, and wheels.
''-[ Button and LEDs ]-
{{ The button constants are used internally for setting and testing button modes.
The LED constansts can be used for turning the LEDs on and off and for setting intensity, blinking, and colors. }}
'Button constants:
RST_ENA = $02 'Bit selector for button reset enable.
LED_ENA = $01 'Bit selector for button reset LED indicator enable.
'LED constants:
OFF = 0 'Applies to all LEDs.
RED = $f0 'Apply to red/green LEDs only...
ORANGE = $b4
YELLOW = $78
CHARTREUSE = $4b
GREEN = $0f
DIM_RED = $70
DIM_GREEN = $07
BLINK_RED = $f1
BLINK_GREEN = $1f
ALT_RED_GREEN = $ff
BLUE = $f0 'Apply to power LED only...
BLINK_BLUE = $f1
DIM_BLUE = $70
''-[ Tone generator ]-
{{ These constants are used to set the voices in the tone player and to form its commands. }}
#0, SQU, SAW, TRI, SIN 'Square, sawtooth, triangle, sine.
#1, STOP, PAUSE, PLAY 'Tone player immediate commands.
#0, TONE, VOLU, SYNC, PAUS 'Tone player sequence commands.
''-[ Result array indices ]-
{{ These constants can be used to reference the various values in the Results array. }}
ADC_VSS = 0
ADC_VDD = 1
ADC_5V = 2
ADC_5V_DIV = 3
ADC_VBAT = 4
ADC_VTRIP = 5 ' S2 Only - Motor Over Current Reference Voltage
ADC_VMOT = 5 ' S3 Only - Motor Supply Voltage
ADC_IDD = 6
ADC_RIGHT_LGT = 7
ADC_CENTER_LGT = 8
ADC_LEFT_LGT = 9
ADC_RIGHT_LIN = 10
ADC_LEFT_LIN = 11
ADC_P6 = 12
ADC_P7 = 13
ADC_IMOT = 14
ADC_IDLER = 15
CNT_IDLER = 16
LED_BYTES = 18 'and 19
TIMER = 20 'and 21
BUTTON_CNT = 22
''-[ Propeller pins ]-
{{ Port names for pins A0 through A31. }}
P0 = 0 'Hacker ports 0 - 5.
P1 = 1
P2 = 2
P3 = 3
P4 = 4
P5 = 5
OBS_TX_LEFT = 6 'Output to left obstacle IRED.
LED_DATA = 7 'Output to LED shift register data pin.
LED_CLK = 8 'Output to LED shift register clock pin.
MIC_ADC_OUT = 9 'Output (feedback) for microphone sigma-delta ADC.
MIC_ADC_IN = 10 'Input for microphone sigma-delta ADC.
BUTTON = 11 'Input for pushbutton.
IDLER_TX = 12 'Output to idler wheel encoder IRED.
MOT_LEFT_ENC = 13 'Input from left motor encoder.
MOT_RIGHT_ENC = 14 'Input from right motor encoder.
OBS_TX_RIGHT = 15 'Output to right obstacle IRED.
MOT_LEFT_DIR = 16 'Output to left motor controller direction pin.
MOT_RIGHT_DIR = 17 'Output to right motor controller direction pin.
MOT_LEFT_PWM = 18 'Output to left motor controller PWM pin.
MOT_RIGHT_PWM = 19 'Output to right motor controller PWM pin.
OBS_RX = 20 'Input from obstacle detector IR receiver.
SPEAKER = 21 'Output to speaker amplifier.
MUX0 = 22 'Outputs to analog multiplexer address pins.
MUX1 = 23
MUX2 = 24
MUX3 = 25
_MUX_ADC_OUT = 26 'Output (feedback) from main sigma-delta ADC.
_MUX_ADC_IN = 27 'Input to main sigma-delta ADC.
SCL = 28 'Output clock to EEPROMs.
SDA = 29 'Input/Output data from/to EEPROMs.
TX = 30 'Output to RS232.
RX = 31 'Input from RS232.
''-[ ADC constants ]-
{{ These are the analog multiplexer addresses controlled by pins MUX0 to MUX3. }}
_MUX_IMOT = 0 'Motor current.
_MUX_VTRIP = 1 '
_MUX_VBAT = 2 'Battery voltage.
_MUX_IDLER = 3 'Idler encoder.
_MUX_VSS = 4 'Vss reference.
_MUX_5V_DIV = 5 '
_MUX_5V = 6 '+5V reference.
_MUX_VDD = 7 '+3.3V reference.
_MUX_P7 = 8 'Hacker port P7 analog input.
_MUX_RIGHT_LGT = 9 'Right light sensor.
_MUX_CENTER_LGT = 10 'Center light sensor.
_MUX_P6 = 11 'Hacker port P6 analog input.
_MUX_IDD = 12 'Vdd current for S2 / Gnd for S3
_MUX_LEFT_LGT = 13 'Left ligth sensor.
_MUX_RIGHT_LIN = 14 'Right line sensor.
_MUX_LEFT_LIN = 15 'Left line sensor.
_VSS = _MUX_VSS << 12 | ADC_VSS << 8
_VDD = _MUX_VDD << 12 | ADC_VDD << 8
_5V = _MUX_5V << 12 | ADC_5V << 8
_5V_DIV = _MUX_5V_DIV << 12 | ADC_5V_DIV << 8
_VBAT = _MUX_VBAT << 12 | ADC_VBAT << 8
_IDD = _MUX_IDD << 12 | ADC_IDD << 8
_IMOT = _MUX_IMOT << 12 | ADC_IMOT << 8
_VTRIP = _MUX_VTRIP << 12 | ADC_VTRIP << 8
_IDLER = _MUX_IDLER << 12 | ADC_IDLER << 8
_RIGHT_LGT = _MUX_RIGHT_LGT << 12 | ADC_RIGHT_LGT << 8
_LEFT_LGT = _MUX_LEFT_LGT << 12 | ADC_LEFT_LGT << 8
_CENTER_LGT = _MUX_CENTER_LGT << 12 | ADC_CENTER_LGT << 8
_RIGHT_LIN = _MUX_RIGHT_LIN << 12 | ADC_RIGHT_LIN << 8
_LEFT_LIN = _MUX_LEFT_LIN << 12 | ADC_LEFT_LIN << 8
_P6 = _MUX_P6 << 12 | ADC_P6 << 8
_P7 = _MUX_P7 << 12 | ADC_P7 << 8
''-[ EEPROM addresses ]-
EE_BASE = 0 'Base address for EEPROM data area.
EE_RESET_CNT = EE_BASE + 0 '[1 byte] Reset count address.
EE_WHEEL_CALIB = EE_BASE + 1 '[5 bytes] Wheel calibration data.
EE_LIGHT_CALIB = EE_BASE + 6 '[4 bytes] Light sensor calibration data.
EE_LINE_THLD = EE_BASE + 10 '[2 bytes] Line sensor threshold data.
EE_OBSTACLE_THLD = EE_BASE + 12 '[2 bytes] Obstacle threshold data.
EE_USER_AREA = EE_BASE + $400 'Beginning of unreserved user area.
''-[ ADC Soak times ]-
_SOAK_1us = 0 << 4
_SOAK_2us = 1 << 4
_SOAK_4us = 2 << 4
_SOAK_8us = 3 << 4
_SOAK_16us = 4 << 4
_SOAK_32us = 5 << 4
_SOAK_64us = 6 << 4
_SOAK_128us = 7 << 4
_SOAK_256us = 8 << 4
_SOAK_512us = 9 << 4
_SOAK_1ms = 10 << 4
_SOAK_2ms = 11 << 4
_SOAK_4ms = 12 << 4
_SOAK_8ms = 13 << 4
_SOAK_16ms = 14 << 4
_SOAK_32ms = 15 << 4
''-[ Filter values ]-
_LPF_NONE = 0 << 1
_LPF_1ms = 1 << 1
_LPF_2ms = 2 << 1
_LPF_4ms = 3 << 1
_LPF_8ms = 4 << 1
_LPF_16ms = 5 << 1
_LPF_32ms = 6 << 1
_LPF_64ms = 7 << 1
''-[ Reference values ]-
_REF_3V3 = 0
_REF_5V0 = 1
''-[ Default values ]-
{{ These values are assigned to their respective variables on startup, unless overriding values are stored
in EEPROM. }}
DEFAULT_FULL_CIRCLE = 955
DEFAULT_WHEEL_SPACE = 153
DEFAULT_LIGHT_SCALE = 0
DEFAULT_LINE_THLD = 32
DEFAULT_OBSTACLE_THLD = 1
''-[ Motor constants ]-
{{ Command, status bits, and indices into the motor debug array. }}
'Command bits:
MOT_IMM = %001 'Sets immediate (preemptive) mode for motor command.
MOT_CONT = %010 'Sets continuous (non-distance) mode for motor command.
MOT_TIMED = %100 'Sets timeout mode for motor command.
'Status bits:
MOT_RUNNING = %01
MOT_STOPPED = %00
'Debug indices:
'These are indices into the motor debug array.
'Offsets are in bytes counting from @Motor_stat.
'The prefix indicates size of each value (Byte, Word, Long)
L_ALL_VEL = 4 'All four control velocities.
B_TARG_VEL = 4 'Target velocity.
B_CUR_VEL = 5 'Current (measured) velocity.
B_END_VEL = 6 'End velocity for this stroke.
B_MAX_VEL = 7 'Maximum velocity for this stroke.
L_BOTH_DIST = 8 'Both left and right stroke distances.
W_RIGHT_DIST = 8 'Right stroke distance.
W_LEFT_DIST = 10 'Left stroke distance.
L_RIGHT_COUNT = 12 'Right coordinated countdown value.
L_LEFT_COUNT = 16 'Left coordinated countdown value.
L_DOM = 20 'Dominant distance and count.
W_DOM_DIST = 20 'Total distance for dominant wheel to travel.
W_DOM_COUNT = 22 'Distance the dominant wheel has traveled.
''-[ Hardware Model Number Constannts ]-
MODEL_S2 = 0
MODEL_S3 = 1
''=======[ Public Spin methods... ]===============================================
''
''-------[ Start and stop methods... ]--------------------------------------------
''
'' Start and stop methods are used for starting individual cogs or stopping all
'' of them at once.
PUB start | i, register
'' Main start routine for scribbler object. Stops ALL cogs, so
'' IT MUST BE CALLED FIRST, before starting other cogs.
''
'' Example: scribbler.start 'Start scribbler object.
stop_all
DIRA[MUX3..MUX0]~~ ' Detect the robot hardware model number.
OUTA[MUX3..MUX0] := _MUX_IDD ' Set the mux address to the 3V3_I_ANG input.
DIRA[_MUX_ADC_IN.._MUX_ADC_OUT]~ ' Set both ADC pins to inputs.
waitcnt(cnt + clkfreq/10) ' Wait for the input to settle.
if INA[_MUX_ADC_IN]
Model_number := MODEL_S2
else
Model_number := MODEL_S3
results_addr := @Results
wordfill(results_addr, 0, 40)
seq_addr := @Adc_sequence
if (Adc_cog := cognew(@adc_all, Model_number << 2) + 1)
outa := constant(1 << SCL)
dira := constant(1 << SPEAKER | 1 << SCL)
_i2c_stop
if (Reset_count := _ee_rdbyte(EE_RESET_CNT))
_ee_wrbyte(EE_RESET_CNT, 0)
if Reset_count > 8
Reset_count~
read_wheel_calibration
read_light_calibration
read_line_threshold
set_led(POWER, BLUE)
if get_model_s3 ' Configure the battery charger.
register := _BQ2495_rdbyte (0) ' Read the charger source control register.
register |= %0_1110_000 ' Set Vin(min) to 5V.
register &= %0_1111_111 ' Clear HiZ disable bit.
_BQ2495_wrbyte(0, register) ' Write the updated register back.
_BQ2495_wrbyte(1, $71) ' OTG Enable, Charge Enable, Watch Dog Reset, Vsys Min 3.0V.
_BQ2495_wrbyte(2, $5C) ' Ichrg = 1984 mA.
_BQ2495_wrbyte(3, $30) ' I PreChrg 512 mA, I term 128 mA.
_BQ2495_wrbyte(4, $B2) ' Vchrg 4.208, Bat Low 3.0V.
_BQ2495_wrbyte(5, $8E) ' Charge Term En, WatchDog Dis, Chg Tmr 20 hours.
_BQ2495_wrbyte(6, $8A) ' Boost = 5.062V, Bhot 65C, Treg 100C.
_BQ2495_wrbyte(7, $48) ' TMR2x En, BATFET On, Disable Interupts.
delay_tenths(5)
return true
else
return false
PUB start_motors
'' Start motor control cog.
''
'' Example: scribbler.start_motors 'Start the motor controller.
ifnot (Motor_cog)
Motor_cmd~~
midler_addr := @Results + (CNT_IDLER << 1)
result := (Motor_cog := cognew(@motor_driver, @Motor_cmd) + 1) > 0
repeat while Motor_cmd
In_path~
here_is(0, 0)
heading_is(Qtr_circle)
set_speed(7)
PUB start_tones
'' Start tone sequencer.
''
'' Example: scribbler.start_tones 'Start the tone sequencer/generator.
ifnot (Tone_cog)
wordfill(@Tone_queue, 0, _TONE_Q_SIZE + 4)
dttime := clkfreq / $1_0000 * 2
queue_addr := @Tone_queue
result := (Tone_cog := cognew(@tone_seq, 0) + 1) > 0
command_tone(PLAY)
PUB start_mic_env
'' Start microphone Envelope detector.
''
'' Example: scribbler.start_mic_env 'Start the microphone Envelope detector.
ifnot (Mic_cog)
mic_dt := clkfreq / 8000
mic_cyc := 8000 / 20
return (Mic_cog := cognew(@env_det, @Envelope) + 1) > 0
PUB stop_all
'' Stop ALL cogs.
''
'' Example: scribbler.stop_all 'Stop all scribbler cogs.
if (Adc_cog)
cogstop(Adc_cog~ - 1)
if (Tone_cog)
cogstop(Tone_cog~ - 1)
if (Mic_cog)
cogstop(Mic_cog~ - 1)
if (Motor_cog)
cogstop(Motor_cog~ - 1)
{{
---------[ Microphone methods... ]---------------------------------------------
Microphone methods provide data from the scribbler's built-in microphone.
}}
PUB get_mic_env
'' Get the average loudness (Envelope) value of the microphone input.
''
'' Example: loudness := scribbler.get_mic_env 'Set loudness equal to current mic level.
return Envelope
{{
---------[ Drawing methods... ]---------------------------------------------------
Drawing methods can be used for drawing with the scribbler or for any application
that requires keeping track of the robot's position and heading.
}}
PUB begin_path
'' Begin a path of connected movements.
''
'' Example: scribbler.begin_path
ifnot (In_path)
In_path := 1
PUB end_path
'' Output the last movement in the path, if there is one, and end the path.
'' NOTE: Omitting this statement may cause the last path segment not to be drawn.
''
'' Example: scribbler.end_path
if (In_path == 2)
run_motors(0, Path_Ldist, Path_Rdist, Path_time, Path_max_spd, 0)
In_path~
PUB set_speed(spd)
'' Set the speed (0 - 15) for the drawing methods, along with the "go_", "turn_",
'' and "arc_" motion methods.
''
'' Example: set_speed(7) 'Set the speed to half of maximum velocity.
Current_spd := spd
PUB move_to(x, y)
'' Move directly to the point (x, y).
'' Units are approximately 0.5mm.
''
'' Example: scribbler.move_to(1000, 50) 'Move to a point 500 mm to the right and 25mm above the origin.
move_by(x - Current_x, y - Current_y)
PUB arc_to(x, y, radius)
'' Move to the point (x, y) via an arc of the specified radius (+radius is CCW; -radius is CW).
'' The Cartesian distance from the current location to the target position must be no more than
'' 2 * radius. If it's greater than that, the robot will move in a straight line to the target
'' position first to make up the difference, then perform the arc.
'' Units are approximately 0.5mm.
''
'' Example: scribbler.arc_to(1000, 50, -100) 'Move to the point 500 mm to the right and 25mm above the origin
'' ' in a clockwise arc of radius 25mm.
arc_by(x - Current_x, y - Current_y, radius)
PUB move_by(dx, dy) | angle
'' Move from the current location by the displacement (dx, dy).
'' Units are approximately 0.5mm.
''
'' Example: scribbler.move_by(100, 50) 'Move to a point 50 mm to the right and 25mm above the current location.
turn_to(angle := _atan2(dy, dx))
go_forward(^^(dx * dx + dy * dy))
here_is(Current_x + dx, Current_y + dy)
PUB arc_by(dx, dy, radius) | dist2, dist, diam, half_angle, tilt
'' Move from the current location byt the displacement (dx, dy) via an arc of the specified radius
'' (+radius is CCW; -radius is CW). The Cartesian length of the displacement must be no more than
'' 2 * radius. If it's greater than that, the robot will move in a straight line to the target
'' position first to make up the difference, then perform the arc. Units are approximately 0.5mm.
''
'' Example: scribbler.arc_by(50, 50, -100) 'Move to the point 25 mm to the right of and 25mm above the
'' ' current location in a clockwise arc of radius 50mm.
dist2 := dx * dx + dy * dy
dist := ^^dist2
tilt := _atan2(dy, dx)
diam := ||radius << 1
if (dist > diam)
turn_to(tilt)
go_forward(dist - diam)
dist := diam
dist2 := dist * dist
half_angle := _atan2(dist >> 1, ^^(radius * radius - (dist2 >> 2)))
if (radius < 0)
half_angle := - half_angle
turn_to(tilt - half_angle)
arc(half_angle << 1, radius)
here_is(Current_x + dx, Current_y + dy)
heading_is(tilt + half_angle)
PUB align_with(heading) | dw
'' Turn so that the robot is pointed parallel to the desired heading (scribbler angle units),
'' either in the selected direction (returns 1) or opposite the selected direction
'' (returns -1), whichever requires the shortest turn to achieve.
''
'' Example: dir := scribbler.align_with(150) 'Point the robot to angle 150 (scribbler angle units)
'' ' or opposite that angle. Set dir to ±1, accordingly.
dw := (heading - Current_w) // Full_circle
dw += Full_circle & (dw < 0)
if (dw > Half_circle)
dw -= Full_circle
if (||dw =< Qtr_circle)
result := 1
else
dw := dw - Half_circle + Full_circle & (dw < 0)
result := -1
turn(dw)
heading_is(Current_w + dw)
PUB turn_to_deg(heading)
'' Turn the robot to the desired heading (degrees).
''
'' Example: scribbler.turn_to(135) 'Point the robot to an angle of 135 degrees.
turn_to(heading * Full_circle / 360)
PUB turn_to(heading)
'' Turn the robot to the desired heading (scribbler angle units).
''
'' Example: scribbler.turn_to(500) 'Point the robot to an angle of 500 scribbler angle units.
turn_by(heading - Current_w)
PUB turn_by_deg(dw)
'' Turn the robot by the desired amount (degrees). +dw is CCW; -dw is CW.
'' If the net turn angle is greater than 180 degrees, the shorter rotation
'' in the opposite direction is used instead.
''
'' Example: scribbler.turn_by_deg(90) 'Rotate the robot by an angle of 90 degrees CCW.
turn_by(dw * Full_circle / 360)
PUB turn_by(dw)
'' Turn the robot by the desired amount (scribbler angle units). +dw is CCW; -dw is CW.
'' If the net turn angle is greater than Full_circle / 2, the shorter rotation
'' in the opposite direction is used instead.
''
'' Example: scribbler.turn_by(500) 'Rotate the robot by an angle of 500 scribbler angle units CCW.
dw //= Full_circle
dw += Full_circle & (dw < 0)
if (dw > Half_circle)
dw -= Full_circle
turn(dw)
heading_is(Current_w + dw)
return 1
PUB here_is(x, y)
'' Reset the current position to (x,y). Units are approximately 0.5mm.
''
'' Example: scribbler.here_is(0, 0) 'Reset the origin to the current location.
Current_x := x
Current_y := y
PUB heading_is_deg(w)
'' Reset the current heading to w degrees.
''
'' Example: scribbler.heading_is_deg(90) 'Reset the current heading to 90 degrees.
heading_is(w * Full_circle / 360)
PUB heading_is(w)
'' Reset the current heading to w.
''
'' Example: scribbler.heading_is(567) 'Reset the current heading to 567.
Current_w := w // Full_circle
Current_w += Full_circle & (Current_w < 0)
{{
---------[ Motion methods... ]-------------------------------------------------
Motion methods control the movement of the scribbler robot. MOTION METHODS DO NOT
KEEP TRACK OF THE SCRIBBLERS'S POSITION AND HEADING, unless called from one of the
drawing methods. As such, they should NOT be mixed with calls to drawing
methods.
}}
PUB read_wheel_calibration | circle, space
'' Read calibration values from EEPROM, and use them if they're reasonable.
'' Returns a packed long containing the calibration values. If no valid values
'' exist in EEPROM, sets (and returns) the default values.
''
'' Example: scribbler.read__wheel_calibration 'Get previously-written wheel calibration values.
if (_ee_rdblock(@circle, EE_WHEEL_CALIB, 4))
space := circle >> 16
circle &= $ffff
if (circle > 900 and circle < 1000 and space > 100 and space < 200)
return set_wheel_calibration(circle, space)
return default_wheel_calibration
PUB write_wheel_calibration
'' Write current wheel calibration values to EEPROM.
'' Returns true on success, false on failure.
''
'' Example: scribbler.write_wheel_calibration 'Write Full_circle and Wheel_space to EEPROM.
return _ee_wrblock(@Full_circle, EE_WHEEL_CALIB, 4)
PUB default_wheel_calibration
'' Restore calibration to default values. DOES NOT SAVE IN EEPROM.
'' This method is optional unless read_calibration or set_calibration
'' have been called.
''
'' Example: scribbler.default_calibration 'Restore calibration defaults.
return set_wheel_calibration(DEFAULT_FULL_CIRCLE, DEFAULT_WHEEL_SPACE)
PUB set_wheel_calibration(circle, space)
'' Set calibration values to method's arguments, if they're reasonable.
'' DOES NOT SAVE IN EEPROM.
''
'' Example: scribbler.set_wheel_calibration(960, 160) 'Set Full_circle to 960 and
'' 'Wheel_space to 160.
Full_circle := circle
Wheel_space := space
_compute_calibration
return get_wheel_calibration
PUB get_wheel_calibration
'' Gets current calibration values: Full_circle in top 16 bits;
'' Wheel_space in bottom 16 bits.
return Full_circle << 16 | Wheel_space
PUB go_left(dist)
'' Turn left and go forward from there by the indicated distance. Units are
'' approximately 0.5mm.
''
'' Example: scribbler.go_left(500) 'Turn left and move forward 250mm.
turn_deg(90)
go_forward(dist)
PUB go_right(dist)
'' Turn right and go forward from there by the indicated distance. Units are
'' approximately 0.5mm.
''
'' Example: scribbler.go_right(500) 'Turn right and move forward 250mm.
turn_deg(-90)
go_forward(dist)
PUB go_forward(dist)
'' Go forward by the indicated distance. Units are approximately 0.5mm.
''
'' Example: scribbler.go_forward(500) 'Move forward 250mm.
if (||dist == FOREVER)
move(100, 100, 0, Current_spd, 1)
else
move(dist, dist, 0, Current_spd, 0)
PUB go_back(dist)
'' Go backward by the indicated distance. Units are approximately 0.5mm.
''
'' Example: scribbler.go_back(500) 'Move back 250mm.
if (||dist == FOREVER)
move(-100, -100, 0, Current_spd, 1)
else
move(-dist, -dist, 0, Current_spd, 0)
PUB turn_deg(ccw_degrees)
'' Turn in place counter-clockwise by the indicated number of degrees.
'' Negative values will turn clockwise.
''
'' Example: scribbler.turn_deg(-90) 'Turn right.
arc_deg(ccw_degrees, 0)
PUB arc_deg(ccw_degrees, radius) | r, l
'' Move in a counter-clockwise arc of the indicated radius by the specified
'' number of degrees. Radius units are approximately 0.5mm. Negative angles
'' result in a clockwise arc.
''
'' Example: scribbler.arc_deg(90, 500) 'Make a sweeping left turn with a radius of 250mm.
arc(Full_circle * ccw_degrees / 360, radius)
PUB turn(ccw_units)
'' Turn in place counter-clockwise by the indicated number of degrees.
'' Negative values will turn clockwise.
''
'' Example: scribbler.turn(-50) 'Turn a bit to the right by 50 scribbler angle units.
arc(ccw_units, 0)
PUB arc(ccw_units, radius) | r, l
'' Move in a counter-clockwise arc of the indicated radius by the specified
'' number of scribbler angle units. Radius units are approximately 0.5mm. Negative angles
'' result in a clockwise arc.
''
'' Example: scribbler.arc(100, 50) 'Arc a bit to the left by 100 scribbler angle units with
'' ' a 25mm radius.
r := ccw_units * (radius + WHEEL_SPACE) / WHEEL_SPACE
l := ccw_units * (radius - WHEEL_SPACE) / WHEEL_SPACE
move(l, r, 0, Current_spd, 0)
PUB turn_deg_now(ccw_degrees)
'' Turn in place counter-clockwise by the indicated number of degrees.
'' Negative values will turn clockwise.
'' Preempts current motion in progress.
''
'' Example: scribbler.turn_deg_now(-90) 'Immediate turn right.
arc_deg_now(ccw_degrees, 0)
PUB arc_deg_now(ccw_degrees, radius) | r, l
'' Move in a counter-clockwise arc of the indicated radius by the specified
'' number of degrees. Radius units are approximately 0.5mm. Negative angles
'' result in a clockwise arc.
'' Preempts current motion in progress.
''
'' Example: scribbler.arc_deg_now(90, 500) 'Make an immediate sweeping left turn
'' 'with a radius of 250mm.
arc_now(Full_circle * ccw_degrees / 360, radius)
PUB turn_now(ccw_units)
'' Turn in place counter-clockwise by the indicated number of degrees.
'' Negative values will turn clockwise.
'' Preempts current motion in progress.
''
'' Example: scribbler.turn_now(-50) 'Immediately turn a bit to the right by 50 scribbler angle units.
arc_now(ccw_units, 0)
PUB arc_now(ccw_units, radius) | r, l
'' Move in a counter-clockwise arc of the indicated radius by the specified
'' number of scribbler angle units. Radius units are approximately 0.5mm. Negative angles
'' result in a clockwise arc.
'' Preempts current motion in progress.
''
'' Example: scribbler.arc_now(100, 50) 'Immediately arc a bit to the left by 100 scribbler angle
'' 'units with a 25mm radius.
r := ccw_units * (radius + WHEEL_SPACE) / WHEEL_SPACE
l := ccw_units * (radius - WHEEL_SPACE) / WHEEL_SPACE
move_now(l, r, 0, Current_spd, 0)
PUB move(left_distance, right_distance, move_time, max_speed, no_stop) | max_d, max_pd, max_rvel, max_lvel, end_spd
'' Base-level non-reactive user move routine. Does not interrupt motion in progress.
'' If called during path construction, velocities will blend. If no path, velocity ramps to zero at end.
'' This method may not return right away if it has to wait for current motion to complete.
''
'' left_distance: Amount to move left wheel (-32767 - 32767) in 0.5mm (approx.) increments.
'' right_distance: Amount to move right wheel (-32767 - 32767) in 0.5mm (approx.) increments.
'' move_time: If non-zero, time (ms) after which to stop, regardless of distance traveled.
'' max_speed (0 - 15): Maximum speed (after ramping).
'' no_stop: If non-zero, keep running, regardless of distance traveled unless/until timeout or a preemptive change.
''
'' Example: scribbler.move(10000, 5000, 10000, 7, 0) 'Move in a clockwise arc for 10000/2 mm on outside, or until 10 seconds elapse,
'' ' whichever occurs first, at half speed.
left_distance := -32767 #> left_distance <# 32767
right_distance := -32767 #> right_distance <# 32767
max_speed := 0 #> max_speed <# 15
Current_spd := max_speed
if (In_path == 2)
if ((left_distance ^ Path_Ldist) & $8000 or (right_distance ^ Path_Rdist) & $8000)
end_spd~
else
max_d := ||left_distance #> ||right_distance
max_pd := ||Path_Ldist #> ||Path_Rdist
max_rvel := ((||right_distance * max_speed + (max_d >> 1))/ max_d <# (||Path_Rdist * Path_max_spd + (max_pd >> 1))/ max_pd) {
} * max_d / (||right_distance #> 1)
max_lvel := ((||left_distance * max_speed + (max_d >> 1))/ max_d <# (||Path_Ldist * Path_max_spd + (max_pd >> 1))/ max_pd) {
} * max_d / (||left_distance #> 1)
end_spd := max_rvel <# max_lvel
run_motors(0, Path_Ldist, Path_Rdist, Path_time, Path_max_spd, end_spd)
result := end_spd
if (In_path => 1)
if (no_stop and move_time == 0)
run_motors(MOT_CONT, left_distance, right_distance, move_time, max_speed, 0)
In_path~
else
Path_Ldist := left_distance
Path_Rdist := right_distance
Path_time := move_time
Path_max_spd := max_speed
In_path := 2
else
run_motors(MOT_CONT & (no_stop <> 0), left_distance, right_distance, move_time, max_speed, 0)
PUB wheels_now(left_velocity, right_velocity, move_time)
'' Set the wheel speeds preemptively to left_velocity and right_velocity (-255 to 255).
'' If move_time > 0, time out after move_time ms.
'' Interrupts any movement in progress and deletes all path information.
'' This method always returns immediately.
''
'' Example: scribbler.wheels_now(-255, 255, 5000) 'Turn left, in place, at maximum speed, for five seconds.
move_now(left_velocity, right_velocity, move_time, (||left_velocity #> ||right_velocity <# 255) >> 4, 1)
PUB move_now(left_distance, right_distance, move_time, max_speed, no_stop)
'' Base-level preemptive user routine for reactive movements (e.g. for line following).
'' Interrupts any movement in progress and deletes all path information.
'' This method always returns immediately.
''
'' left_distance: Amount to move left wheel (-32767 - 32767) in 0.5mm (approx.) increments.
'' right_distance: Amount to move right wheel (-32767 - 32767) in 0.5mm (approx.) increments.
'' move_time: If non-zero, time (ms) after which to stop, regardless of distance traveled.
'' max_speed (0 - 15): Maximum speed (after ramping).
'' no_stop: If non-zero, keep running, regardless of distance traveled unless/until timeout or another preemptive change.
''
'' Example: scribbler.move_now(1000, -1000, 0, 15, 1) 'Rotate in place clockwise at full speed until preempted.
In_path~
run_motors(MOT_IMM | MOT_CONT & (no_stop <> 0), left_distance, right_distance, move_time, max_speed, 0)
PUB stop_now
'' Stops movement immediately and deletes all path information.
''
'' Example: scribbler.stop_now.
In_path~
run_motors(MOT_IMM, 0, 0, 0, 0, 0)
PUB wait_stop
'' Wait for all current and pending motions to complete.
''
'' Example: scribbler.wait_stop
repeat while moving
PUB stalled | stat, mvel, ivel, itime, vstall, istall
'' Checks whether the scribbler is stalled by testing both the motor current
'' and the activitity of the idler wheel encoder.
'' Returns true if stalled; false if not.
''
'' Example: repeat until scribbler.stalled 'Execute the repeat block as long as the bot is not stalled.
if ((stat := Motor_stat) & $03)
mvel := ||(stat ~> 24 + stat << 8 ~> 24)
itime := (stat >> 8) & $ff
ifnot (ivel := (stat & $fc) << 3)
ivel := 512 / itime
if (vstall := mvel * 14 / ivel > 20 + Stall_hyst)
Stall_hyst := -2
else
Stall_hyst := 2
istall := get_adc_results(ADC_IMOT) > (75 * get_adc_results(ADC_VBAT)) >> 7
return vstall or istall
PUB moving
'' Return TRUE if motion in progress or pending, FALSE if stopped with no pending motions.
''
'' Example: repeat while scribbler.moving 'Continuously execute the following repeat block until motions are finished.
return Motor_stat & $03 <> 0
PUB motion
'' Return current motion status:
''
''' 31 24 23 16 15 8 7 2 1 0
''' ┌─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┬─┐
''' │± Left wheel │± Right wheel │ Idler timer │ Idler spd │Mov│ , where
''' └─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┴─┘
''
'' Left wheel and right wheel are signed, twos complement eight bit velocity values,
'' Idler timer is the time in 1/10 second since the last idler edge,
'' Idler spd is an unsigned six-bit velocity value, and
'' Mov is non-zero iff one or more motors are turning.
'' Left and right wheel velocities are instanteous encoder counts over a 1/10-second interval.
'' Idler wheel wheel velocity is updated every 1/10 second and represents the idler encoder count during the last 1.6 seconds.
''
'' Example: left_vel := scribbler.motion ~> 24 'Get the current left wheel velocity as a signed 32-bit value.
return Motor_stat
PUB motion_addr
'' Return the address of the status and debug array.
''
'' Example: longmove(@my_stats, scribbler.motion_addr, 6) 'Copy all status data to the local array my_stats.
return @Motor_stat
PUB move_ready
'' Return TRUE if a new motion command can be accepted without waiting, FALSE if a command is still pending.
''
'' Example: repeat until scribbler.move_ready 'Continuously execute the following repeat block until a new move can be accepted.
return Motor_cmd == 0
PUB run_motors(command, left_distance, right_distance, timeout, max_speed, end_speed)
{{ Base level motor activation routine. Normally, this method is not called by the user but is called by the
several convenience methods available to the user.
}}
''
'' `command: the OR of any or all of the following:
''
'' MOT_IMM: Commanded motion starts immediately, without wating for prior motion to finish.
'' MOT_CONT: Commanded motion will continue to run at wheel ratio given by left and right distances,
'' even after distances are covered.
''
'' `left_distance, `right_distance (-32767 to 32767):
''
''
'' The distances to be covered by the left and right wheels, respectively. Units are approximately 0.5mm.
''
'' `timeout (0 - 65535): If non-zero, time limit (ms) after which motion stops, regardless of distance covered.