Roomba/oopic_code.txt at master · FallDownT/Roomba · GitHub

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


Dim rightmotor As New oDCMotor
Dim leftmotor As New oDCMotor
Dim serialL As New oSerialX
Dim serialR As New oSerialX
Dim rangetx As New oSerialX
Dim servotx As New oSerialX
Dim servo As New oServoX
Dim i As New oByte
Dim state As New oNibble
Dim red As New oLED
Dim yellow As New oLED
Dim green As New oLED
Dim ir As New oA2D
Dim toggle As New oDIO1


'Dim wire As New oWire
'Dim led As New oDIO1


Sub Main()
	ooPIC.Delay = 5000
	Call setup()

	Do


			servo.Position = i
			servotx.Value = servo.Position
			i = i + 4
			Call updatemotors()
			rangetx.Value = ir.Result
			Call updateLEDS()
			toggle.Toggle = 1

	Loop


End Sub

Sub updatemotors()
	rightmotor.Speed = serialR.Value
	leftmotor.Speed = serialL.Value

End Sub

Sub updateLEDS()
	If state = 1
		red.Brightness = 5
		yellow.Brightness = 5
		green.Brightness = 15
		state = 2
	Else If state = 2
		red.Brightness = 5
		yellow.Brightness = 15
		green.Brightness = 5
		state = 3
	Else If state = 3
		red.Brightness = 15
		yellow.Brightness = 5
		green.Brightness = 5
		state = 1
	End If
End Sub


Sub setup()
'led.IOLine = 5
'led.Direction = cvOutput
'wire.Input.Link(ooPIC.Hz1)
'wire.Output.Link(led.State)
'wire.Operate = cvTrue


	green.IOLine = 5
	green.Operate = 1
	yellow.IOLine = 6
	yellow.Operate = 1
	red.IOLine = 7
	red.Operate = 1

	serialR.Baud = cv9600
	serialR.IOLineS = 12
	serialR.IOLineF = 11
	serialR.InvertF = 1
	serialR.Init
	serialR.Operate = 1

	serialL.Baud = cv9600
	serialL.IOLineS = 10
	serialL.IOLineF = 9
	serialL.InvertF = 1
	serialL.Init
	serialL.Operate = 1

	rangetx.Baud = cv9600
	rangetx.IOLineS = 14
	rangetx.IOLineF = 0
	rangetx.Init
	rangetx.Operate = 1

	servotx.Baud = cv9600
	servotx.IOLineS = 13
	servotx.IOLineF = 0
	servotx.Init
	servotx.Operate = 1


	rightmotor.IOLineP = 18
	rightmotor.IOLineB = 24
	rightmotor.IOLineD = 26
	rightmotor.Operate = 1
	rightmotor.Brake = 0
	rightmotor.Invert = 1
	rightmotor.Init

	leftmotor.IOLineP = 17
	leftmotor.IOLineB = 25
	leftmotor.IOLineD = 27
	leftmotor.Operate = 1
	leftmotor.Brake = 0
	leftmotor.Init

	servo.Center=0
	servo.InvertOut=0
	servo.IOLine=8
	servo.Mode=0
	servo.Operate=1
	i = -128
	state = 1

	ir.IOLine = 1
	ir.Operate = cvTrue

	toggle.IOLine = 15
	toggle.State = 1

End Sub