-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
154 lines (105 loc) · 2.89 KB
/
main.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
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
import serial
import math
import time
import threading
ser = serial.Serial('/dev/cu.usbserial-A70365JW', 38400, timeout=1)
def sendDegree(zero,one,two,three,four,five):
message = str(zero)+","+str(one)+","+str(two)+","+str(three)+","+str(four)+","+str(five)+","
ser.write(message)
print(message)
# def open():
lastx = 0
lasty = 20
opengrab = False
movedup = False
rt = 550
def gt():
global opengrab
opengrab = not opengrab
mv(lastx,lasty);
def mt():
global movedup
movedup = not movedup
mv(lastx,lasty);
def quickchange():
mv(0,8)
time.sleep(0.1)
mv(1,7)
def mv(xDistance, yDistance):
print xDistance, yDistance
global lastx
global lasty
lastx = xDistance
lasty = yDistance
b = 7.25
a = 5.75
# b = 18.0
# a = 14.0
originDist = math.sqrt(xDistance**2 + yDistance**2) + 0.00
rotation = math.degrees(math.acos(yDistance / originDist))
if(xDistance < 0):
rotation *= -1;
c = originDist
cosA = (b**2 + c**2 - a**2) / (2*b*c)
# print cosA
angleA = math.degrees(math.acos(cosA))
cosB = (c**2 + a**2 - b**2) / (2*c*a)
angleB = math.degrees(math.acos(cosB))
angleC = 180.0 - angleA - angleB;
# print angleA,angleB,angleC
# print (angleB / 90.0) * 500
# print (angleC / 90.0) * 500
grab = 1000
if opengrab:
grab = 0
if movedup:
angleB += 30
p1 = math.ceil(((rotation / 90)*500 + 500)*100)/100
p2 = math.ceil(((angleB / 90.0) * 500)*100)/100
p3 = math.ceil(((angleC / 90.0) * 500)*100)/100
p4 = math.ceil(((angleA / 90)*500 + 500)*100)/100
p5 = math.ceil(rt*100)/100
p6 = math.ceil(grab*100)/100
sendDegree(p1,p2,p3,p4,p5,p6)
# sendDegree(2,)
time.sleep(2)
mv(0,10)
def serialLog():
while True:
line = ser.readline()
if(len(line) > 0):
print(line)
thr = threading.Thread(target=serialLog)
thr.start() # will run "foo"
def draw():
coords = [];
with open("points.txt") as f:
content = f.readlines()
for i,val in enumerate(content):
coords.append(val.split("\n")[0])
print(coords)
sleeptime = 3;
for i in range(0,(len(coords)/4)):
startx = float(coords[i*4])*-1
starty = float(coords[i*4 + 1])
endx = float(coords[i*4 + 2])*-1
endy = float(coords[i*4 + 3])
mt();
time.sleep(sleeptime)
mv(startx,starty)
time.sleep(sleeptime)
mt();
time.sleep(sleeptime)
mv(endx,endy)
time.sleep(sleeptime)
# def moveSmoothly(xDistance, yDistance):
# originx = lastx
# originy = lasty
# xDis = xDistance - lastx
# yDis = yDistance - lasty
# print yDis
# smoothness = 10
# for i in range(1,smoothness+1):
# mv(originx + xDis*(i/(smoothness*1.0)), originy + yDis*(i/(smoothness*1.0)))
# time.sleep(1.5)
# sendDegree(1,1200)