-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathQ4.bs2
107 lines (88 loc) · 3.5 KB
/
Q4.bs2
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
' Q4.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ I/O Definitions ]-------------------------------------------------
LMotor PIN 13 ' left servo motor
RMotor PIN 12 ' right servo motor
LLinePwr PIN 10 ' left line sensor power
LLineIn PIN 9 ' left line sensor input
RLinePwr PIN 7 ' right line sensor power
RLineIn PIN 8 ' right line sensor input
' -----[ Constants ]-------------------------------------------------------
LFwdFast CON 1000 ' left motor fwd; fast
LFwdSlow CON 800 ' left motor fwd; slow
LStop CON 750 ' left motor stop
LRevSlow CON 700 ' left motor rev; slow
LRevFast CON 500 ' left motor rev; fast
RFwdFast CON 500 ' right motor fwd; fast
RFwdSlow CON 700 ' right motor fwd; slow
RStop CON 750 ' right motor stop
RRevSlow CON 800 ' right motor rev; slow
RRevFast CON 1000 ' right motor rev; fast
' -----[ Variables ]-------------------------------------------------------
pulses VAR Byte ' servo pulses counter
lLine VAR Word ' left sensor raw reading
rLine VAR Word ' right sensor raw reading
lineBits VAR Nib ' decoded sensors value
lbLeft VAR lineBits.BIT1
lbRight VAR lineBits.BIT0
' -----[ Initialization ]--------------------------------------------------
Reset:
LOW LMotor ' initialize motor outputs
LOW RMotor
PAUSE 2000 ' time to disconnect cable
' -----[ Program Code ]----------------------------------------------------
Main:
GOSUB Read_Line_Sensors
' sumo movement
BRANCH lineBits, [Go_Fwd, Spin_Left, Spin_Right, About_Face]
Go_Fwd:
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
GOTO Main
Spin_Left:
FOR pulses = 1 TO 20
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RFwdFast
PAUSE 20
NEXT
GOTO Main
Spin_Right:
FOR pulses = 1 TO 20
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
GOTO Main
About_Face:
FOR pulses = 1 TO 10 ' back up from edge
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
FOR pulses = 1 TO 30 ' turn around
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
FOR pulses = 1 TO 40
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
PAUSE 20
NEXT
END
' -----[ Subroutines ]-----------------------------------------------------
Read_Line_Sensors:
HIGH LLinePwr ' activate sensors
HIGH RLinePwr
HIGH LLineIn ' discharge caps
HIGH RLineIn
PAUSE 1
RCTIME LLineIn, 1, lLine ' read left sensor
RCTIME RLineIn, 1, rLine ' read right sensor
LOW LLinePwr ' deactivate sensors
LOW RLinePwr
' convert readings to bits
LOOKDOWN lLine, >=[1000, 0], lbLeft ' 0 = black, 1 = line
LOOKDOWN rLine, >=[1000, 0], lbRight
RETURN