Skip to content

Commit a58930f

Browse files
koshovkoshov
committed
Kur
1 parent a71ec3b commit a58930f

File tree

3 files changed

+63
-95
lines changed

3 files changed

+63
-95
lines changed

‎event.py

+18-42
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
1-
import state
1+
from math import pi
22
from time import sleep
3+
import state
34

45

56
class Event():
@@ -56,7 +57,7 @@ def transition(self):
5657

5758
class AtHome(Event):
5859
def __init__(self, robot):
59-
self.robot = robot
60+
self.robot = robot
6061

6162
def check(self):
6263
if abs(self.robot.x) < 50 and abs(self.robot.y) < 50:
@@ -81,50 +82,25 @@ def __init__(self, robot, x, y):
8182
self.robot = robot
8283
self.x = x
8384
self.y = y
84-
if self.x < self.robot.x:
85-
self.param_x = -123
86-
else:
87-
self.param_x = 123
8885

89-
if self.y < self.robot.y:
90-
self.param_y = -123
91-
else:
92-
self.param_y = 123
93-
94-
self.bounding_x = self.x + self.param_x
95-
self.bounding_y = self.y + self.param_y
86+
self.bounding_r = (abs(self.x) - abs(self.robot.x))**2 + (abs(self.y) - abs(self.robot.y))**2
87+
self.bounding_x = self.robot.x
88+
self.bounding_y = self.robot.y
9689

9790
def check(self):
98-
# Check if the current robot coordinates are the ones we expect
99-
# print 'Goal at: (%f, %f) we are at: (%f, %f)' % (self.x,self.y,self.robot.x,self.robot.y)
100-
print 'Phi: %f, Target: %f'%(self.robot.phi, self.robot.target_angle)
101-
# print 'angle is %f' %self.robot.phi
102-
if self.robot.phi > self.robot.target_angle:
91+
robot_phi = self.robot.phi % (2*pi)
92+
target_phi = self.robot.target_angle % (2*pi)
93+
# print 'Phi: %f, Target: %f'%(robot_phi, target_phi)
94+
print 'G: %f, %f\nR: %f, %f'%(self.x, self.y, self.robot.x, self.robot.y)
95+
if robot_phi > target_phi + target_phi * 0.1:
10396
self.robot.setSpeeds(5, 4)
104-
elif self.robot.phi < self.robot.target_angle:
97+
elif robot_phi < target_phi - target_phi * 0.1:
10598
self.robot.setSpeeds(4, 5)
10699
else:
107100
self.robot.setSpeeds(5, 5)
108101

109102

110-
if self.param_x < 0:
111-
x_reached = abs(self.bounding_x) > self.robot.x
112-
else:
113-
x_reached = abs(self.bounding_x) < self.robot.x
114-
115-
if self.param_y < 0:
116-
y_reached = abs(self.bounding_y) > self.robot.y
117-
else:
118-
y_reached = abs(self.bounding_y) < self.robot.y
119-
120-
if x_reached or y_reached:
121-
return True
122-
#return not((abs(self.robot.x) <= abs(self.x)+200) and (abs(self.robot.y) <= abs(self.y)+200))
123-
#print 'stop'
124-
return (abs(self.robot.x) - abs(self.x))**2 + (abs(self.robot.y) - abs(self.y))**2 < 200**2
125-
#if ((abs(self.robot.x) <= abs(self.x)+200) and (abs(self.robot.y) <= abs(self.y)+200)):
126-
##Check if the current robot coordinates are the ones we expect
127-
#if(self.robot.x < abs(self.x) + 50) or (self.robot.x > self.x+50)
103+
return (abs(self.robot.x) - abs(self.bounding_x))**2 + (abs(self.robot.y) - abs(self.bounding_y))**2 > self.bounding_r
128104

129105
def call(self):
130106
# self.robot.stop()
@@ -171,15 +147,15 @@ def check(self):
171147
front = 0
172148

173149
if sensor_values is not None:
174-
if self.wall_position == "left":
150+
if self.wall_position == "left":
175151
gain_dir = ["left", "right"]
176152
target_wall = sensor_values[0]
177153
other_wall = sensor_values[5]
178-
else:
154+
else:
179155
gain_dir = ["right", "left"]
180156
target_wall = sensor_values[5]
181157
other_wall = sensor_values[0]
182-
158+
183159
# Escape from nearby wall
184160
if max([sensor_values[0], sensor_values[5]]) > self.THRESHOLD:
185161
if target_wall > other_wall:
@@ -194,8 +170,8 @@ def check(self):
194170
# Move towards wall when loosing it
195171
else:
196172
self.gain = (gain_dir[1], (1-self.THRESHOLD) - target_wall * (1-self.THRESHOLD)/self.THRESHOLD + front/self.robot.FULL_SPEED, front)
197-
return True
198-
173+
return True
174+
199175
return False
200176

201177
def call(self):

‎robot.py

+34-47
Original file line numberDiff line numberDiff line change
@@ -119,8 +119,7 @@ def __init__(self, robot):
119119
class Go_to(Action):
120120
def __init__(self, robot, x, y):
121121
print "Rotating to face goal"
122-
robot.target_angle = atan2(y, x)
123-
print robot.target_angle
122+
robot.target_angle = atan2(y-robot.y, x-robot.x)
124123
robot.rotateTo(robot.target_angle)
125124
print "Done Rotating, now moving towards goal"
126125
temp = robot.phi
@@ -163,64 +162,49 @@ def __init__(self, robot):
163162
self.robot.setSpeeds(5, -5)
164163
self.events = [event.Parallel_completed(robot, [4, 5])]
165164

166-
# class GoHome(Action):
167-
# def __init__(self, robot):
168-
# self.robot = robot
169-
# robot.stop()
170-
# alpha = atan(robot.y / robot.x)
171-
172-
# if ((self.robot.phi - alpha) > pi):
173-
# destination = pi - robot.phi - alpha
174-
# else:
175-
# destination = robot.phi - alpha - pi
176-
177-
# destination = pi - (robot.phi % (2 * pi)) - alpha
178-
# robot.phi = robot.phi % (2 * pi)
179-
# robot.rotateTo(destination)
180-
# self.events = []
181-
182-
# ==== Helper Actions ====
183-
# def goTo(self, x):
184-
# self.resetCounts()
185-
186-
# self.go(self.FULL_SPEED)
187-
# while self.x < x:
188-
# data = self.readCount()
189-
# if len(data) == 2:
190-
# left_n = int(data[0])
191-
# right_n = int(data[1])
192-
# left_d = left_n - self.left_l
193-
# self.left_l = left_n
194-
# right_d = right_n - self.right_l
195-
# self.right_l = right_n
196-
# print "L: %f R: %f"%(left_d, right_d)
197-
# self.setOdometry(left_d, right_d)
198-
199-
# self.stop()
200165

201166
def rotateTo(self, phi):
202-
print "Target: %f"%phi
203-
print "At %f"%self.phi
204-
if phi > 0:
205-
self.setSpeeds(-self.FULL_SPEED, self.FULL_SPEED)
167+
168+
robot_phi = self.phi % (2*pi)
169+
target = phi % (2*pi)
170+
171+
if (robot_phi - target) < 0:
172+
if (target - robot_phi) > pi:
173+
rot = (2*pi) - target - robot_phi
174+
self.setSpeeds(self.FULL_SPEED, -self.FULL_SPEED)
175+
else:
176+
rot = target - robot_phi
177+
self.setSpeeds(-self.FULL_SPEED, self.FULL_SPEED)
206178
else:
207-
self.setSpeeds(self.FULL_SPEED, -self.FULL_SPEED)
179+
if robot_phi - target > pi:
180+
rot = (2*pi) - target - robot_phi
181+
self.setSpeeds(-self.FULL_SPEED, self.FULL_SPEED)
182+
else:
183+
rot = robot_phi - target
184+
self.setSpeeds(self.FULL_SPEED, -self.FULL_SPEED)
208185

209-
start_phi = self.phi
210186

211-
while (phi>0 and self.phi < start_phi + phi) or (phi < 0 and self.phi > start_phi + phi):
187+
# if robot_phi > pi: robot_phi = 2*pi - robot_phi
188+
189+
print "Target: %f"%target
190+
print "At %f"%robot_phi
191+
print "I should rotate by: %f" %rot
192+
193+
while abs(robot_phi - target) > 0.1:
212194
data = self.readCount()
213195
if len(data) == 2:
214196
left_n = int(data[0])
215197
right_n = int(data[1])
198+
216199
left_d = left_n - self.left_l
217200
self.left_l = left_n
218201
right_d = right_n - self.right_l
219202
self.right_l = right_n
220-
# print "L: %f R: %f"%(left_d, right_d)
221203
self.setOdometry(left_d, right_d)
222204

223-
self.stop()
205+
robot_phi = self.phi % (2*pi)
206+
207+
# self.stop()
224208

225209

226210
# ==== Khepera Functions ====
@@ -246,8 +230,11 @@ def validateSensorValue(self, sensorString):
246230
if len(result) < 8:
247231
return None
248232
for value in result:
249-
value = int(value)
250-
if value < 0 or value > 1024:
233+
try:
234+
value = int(value)
235+
if value < 0 or value > 1024:
236+
return None
237+
except ValueError, e:
251238
return None
252239
return result
253240

‎state.py

+11-6
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,19 @@ def __init__(self, robot, pointsArray):
2424
self.setAction()
2525

2626
def nextWaypoint(self):
27-
self.i += 1
28-
if self.i <= len(self.pointsArray):
29-
self.setAction()
27+
self.i += 1
28+
if self.i < len(self.pointsArray):
29+
self.setAction()
30+
else:
31+
print 'Maika ti paidjina'
32+
self.robot.stop()
33+
self.robot.state = State(self.robot)
34+
3035

3136
def setAction(self):
32-
(x, y) = self.pointsArray[self.i]
33-
self.action = self.robot.Go_to(self.robot, x, y)
34-
self.events = [event.Reached_Positon(self.robot, x, y)]
37+
(x, y) = self.pointsArray[self.i]
38+
self.action = self.robot.Go_to(self.robot, x, y)
39+
self.events = [event.Reached_Positon(self.robot, x, y)]
3540

3641
class Moving_To_Target(State):
3742
def __init__(self, robot, x, y):

0 commit comments

Comments
 (0)