-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPlotter.py
129 lines (109 loc) · 3.38 KB
/
Plotter.py
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
# Python library for Cyber Buggies Pen Plotter
from vex import *
def home():
home_Xaxis()
home_Yaxis()
return 0
# ---------------------------------------------------------- #
def home_Xaxis():
# Home Xaxis
print("Homing Xaxis")
Xaxis.set_velocity(50, PERCENT)
Xaxis.spin(REVERSE)
print("Waiting for first stop")
while not Xstop.pressing():
pass
Xaxis.stop()
print("Stopped")
Xaxis.spin_for(FORWARD, 1, TURNS)
print("Moving forward")
Xaxis.set_velocity(20, PERCENT)
Xaxis.spin(REVERSE)
print("Waiting for second stop")
while not Xstop.pressing():
pass
Xaxis.stop()
print("Stopped")
Xaxis.set_position(0,TURNS)
print("Xaxis homing success!")
return 0
# ---------------------------------------------------------- #
def home_Yaxis():
# Home Yaxis
print("Homing Yaxis")
Yaxis.set_velocity(50, PERCENT)
Yaxis.spin(REVERSE)
print("Waiting for first stop")
LeftStopped = False
RightStopped = False
while not(YstopLeft.pressing() and YstopRight.pressing()):
if YstopLeft.pressing() and not(LeftStopped):
Yaxis_motor_a.stop()
LeftStopped = True
print("Left Stopped")
if YstopRight.pressing() and not(RightStopped):
Yaxis_motor_b.stop()
RightStopped = True
print("Right Stopped")
Yaxis.stop()
print("Stopped")
Yaxis.spin_for(FORWARD, 1, TURNS)
print("Moving forward")
Yaxis.set_velocity(20, PERCENT)
Yaxis.spin(REVERSE)
print("Waiting for second stop")
LeftStopped = False
RightStopped = False
while not(YstopLeft.pressing() and YstopRight.pressing()):
if YstopLeft.pressing() and not(LeftStopped):
Yaxis_motor_a.stop()
LeftStopped = True
print("Left Stopped")
if YstopRight.pressing() and not(RightStopped):
Yaxis_motor_b.stop()
RightStopped = True
print("Right Stopped")
Yaxis.stop()
print("Stopped")
Yaxis.set_position(0, TURNS)
print("Yaxis homing success!")
return 0
# ---------------------------------------------------------- #
def set_velocity(value):
Xaxis.set_velocity(value, PERCENT)
Yaxis.set_velocity(value, PERCENT)
return value
# ---------------------------------------------------------- #
def set_zero_pos():
Xaxis.set_position(0, TURNS)
Yaxis.set_position(0, TURNS)
# ---------------------------------------------------------- #
ScaleFactor = 10
def get_ScaleFactor():
global ScaleFactor
return ScaleFactor
def set_ScaleFactor(new):
global ScaleFactor
ScaleFactor = new
return ScaleFactor
# ---------------------------------------------------------- #
def get_position():
global ScaleFactor
x = Xaxis.position(DEGREES) * ScaleFactor
y = Yaxis.position(DEGREES) * ScaleFactor
return x, y
# ---------------------------------------------------------- #
def goto(x, y):
Xaxis.spin_to_position((x * get_ScaleFactor), DEGREES)
Yaxis.spin_to_position((y * get_ScaleFactor), DEGREES)
return x, y
# ---------------------------------------------------------- #
def SetPenDown(input):
if input == True:
return 0
elif input == False:
return 0
else:
print("Invalid input for SetPen")
return 1
# ---------------------------------------------------------- #