-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathspike_pybricks_moveinmap.py
146 lines (134 loc) · 5.24 KB
/
spike_pybricks_moveinmap.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
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
import umath as math
hub = PrimeHub()
class MotorPair:
def __init__(self, port1, port2):
self.motor1 = Motor(port1)
self.motor2 = Motor(port2)
self.timer = StopWatch()
def move_angle(self,amount,speed1,speed2, timeout = 1000):
self.motor1.reset_angle(0)
self.motor2.reset_angle(0)
self.timer.reset()
while abs(self.motor1.angle()) < amount or self.timer.time() < timeout:
while abs(self.motor2.angle()) < amount or self.timer.time() < timeout:
self.motor1.run(speed1)
self.motor2.run(-(speed2))
self.motor1.stop()
self.motor2.stop()
return "succeded"
def move_tank(self,amount, speed1, speed2):
self.motor1.run(speed1)
self.motor2.run(-(speed2))
wait(amount)
self.motor1.stop()
self.motor2.stop()
def start_tank(self, speed1, speed2):
self.motor1.run(speed1)
self.motor2.run(-(speed2))
def stop_tank(self):
self.motor1.run(0)
self.motor2.run(0)
class myMap:
def __init__ (self,start_pos):
self.start_pos = start_pos
pos = [self.start_pos[0], self.start_pos[1], self.start_pos[2]]
self.points = [pos]
def addPoint(self,point):
pos = [point[0], point[1], point[2]]
if not pos in self.points:
self.points += [pos]
class Robot:
def __init__ (self,motorE,motorD,force_sensor_port = None,position = [0,0,0]):
self.position = position
self.map = myMap(self.position)
self.motors = MotorPair(motorE, motorD)
self.hub = PrimeHub()
self.hub.imu.reset_heading(0)
if force_sensor_port:
self.force_sensor = ForceSensor(force_sensor_port)
def pointTo(self, degrees, precision = 1):
dir = self.position[2]
print(range(degrees - precision, degrees + precision))
if(dir < degrees):
while self.hub.imu.heading() < (degrees - precision) or self.hub.imu.heading() > (degrees + precision):
print(self.hub.imu.heading())
self.motors.start_tank(-200,200)
self.motors.stop_tank()
else:
while self.hub.imu.heading() < (degrees - precision) or self.hub.imu.heading() > (degrees + precision):
self.motors.start_tank(200,-200)
self.motors.stop_tank()
print('turning ' + str(degrees) + ' degrees')
self.position[2] = self.hub.imu.heading()
self.map.points[-1][2] = self.position[2]
def calibrateDirTo(self, goal, precision = 1):
dir = self.position[2]
if dir < goal:
while not self.hub.imu.heading() == goal:
real_value = self.hub.imu.heading()
diff = goal - real_value
print(str(real_value) + " missing " + str(diff) + " to turn ")
else:
while not self.hub.imu.heading() == goal:
real_value = self.imu.heading()
diff = real_value - goal
print(str(real_value) + " missing " + str(diff) + " to turn ")
real_value = self.hub.imu.heading()
print('dir calibrated to ' + str(real_value) + ' degrees, the goal was ' + str(goal))
self.position[2] = self.hub.imu.heading()
self.map.points[-1][2] = self.position[2]
def moveX(self,q):
if q:
if q > 0:
self.pointTo(90)
else:
self.pointTo(-90)
self.motors.move(abs(q),'cm', 0, 50)
print('moving ' + str(q) + ' on X')
self.position[0] += q
self.map.addPoint(self.position)
def moveY(self,q):
if q :
if q > 0:
self.pointTo(0)
else:
self.pointTo(178)
self.motors.move(abs(q),'cm', 0, 50)
print('moving ' + str(q) + ' on Y')
self.position[1] += q
self.map.addPoint(self.position)
def goTo(self,x,y):
x = int( x + (-1*self.position[0]) )
y = int( y + (-1*self.position[1]) )
dist = math.sqrt(x**2 + y**2)
if x != 0 and y > 0:
self.pointTo(int(math.degrees(math.atan(x/y))))
elif x > 0 and y < 0:
self.pointTo(90 + int(abs(math.degrees(math.atan(y/x)))))
elif x < 0 and y < 0:
self.pointTo(-90 - int(abs(math.degrees(math.atan(y/x)))))
elif x == 0 and y != 0:
self.moveY(y)
return 1
elif x != 0 and y == 0:
self.moveX(x)
return 1
else:
return 0
self.motors.move_angle(dist,200, 200)
self.position[0] += x
self.position[1] += y
self.map.addPoint(self.position)
return 1
def doRoute(self, pointlist, goandback = 'go'):
for point in pointlist:
self.goTo(point[0],point[1])
if goandback == 'go_comeback':
lista = self.map.points.copy()
lista.reverse()
self.doRoute(lista)