def main(self):
rc = RobotControl()
d_left, d_right = self.get_laser_readings()
print(d_left, d_right)
while True:
rc.move_straight()
d_left, d_right = self.get_laser_readings()
print(d_left, d_right)
if (math.isinf(d_left)) and (math.isinf(d_right)):
break
rc.stop_robot()
class Project:
def __init__(self,speed,time):
self.rc=RobotControl()
self.motion=None
self.speed=speed
self.time=time
self.d=None
def dist(self):
return(self.rc.get_laser(360))
def mov(self):
distance=self.dist()
while True:
while (distance>1):
self.rc.move_straight()
distance=self.dist()
print("Current distance from wall : ", distance)
self.rc.stop_robot()
self.motion=self.where_turn()
self.rc.turn(self.motion,self.speed,self.time)
print("turning ", self.motion)
distance=self.dist()
def where_turn(self):
self.d=self.rc.get_laser_full()
l1=[]
l2=[]
cnt=len(self.d)
i=0
j=cnt/2
while(i<cnt/2):
l1.append(self.d[i])
i=i+1
while(j<cnt):
l2.append(self.d[j])
j=j+1
v1,v2=self.mean(l1,l2)
print("sum of right= ",v1)
print("sum of left= ",v2)
if(v1>v2):
self.d=None
return "clockwise"
else:
self.d=None
return "counter-clockwise"
def mean(self,x,y):
x=np.asarray(x)
y=np.asarray(y)
m1=np.sum(x)
m2=np.sum(y)
return [m1,m2]
class ExamControl:
def __init__(self):
self.rc = RobotControl()
def get_laser_readings(self, left=719, right=0):
self.rc.get_laser(left)
self.rc.get_laser(right)
return self.rc.get_laser(left), self.rc.get_laser(right)
def main(self, left=719, right=0):
while self.rc.get_laser(left) != float("inf") or self.rc.get_laser(
right) != float("inf"):
self.rc.move_straight()
self.rc.stop_robot()
self.rc.get_laser(right)
class ExamControl:
def __init__(self):
self.rc = RobotControl()
def get_laser_readings(self):
self.lf = self.rc.get_laser(719)
self.rf = self.rc.get_laser(0)
return self.lf, self.rf
def main(self):
self.l, self.r = self.get_laser_readings()
#print (self.l,self.r)
while not math.isinf(self.l) or not math.isinf(self.r):
self.rc.move_straight()
self.l, self.r = self.get_laser_readings()
self.rc.stop_robot()
class robotMaze:
def __init__(self, moveDirection, turnDirection, speed):
self.RobotControl = RobotControl()
self.moveDirection = moveDirection
self.turnDirection = turnDirection
self.speed = speed
self.timeTurn = 4.80 #Set to get as close to 90 degrees as possible
self.left = 0 #Sets leftmost laser value
self.middle = 360 #sets middle laser value
self.right = 719 #sets rightmost laser value
def solveMaze(self):
while ((self.RobotControl.get_laser(self.left) != math.inf) &
(self.RobotControl.get_laser(self.right) != math.inf)):
self.wallMove()
self.checkBounds()
self.RobotControl.move_straight()
self.RobotControl.stop_robot()
def wallMove(self):
distance = self.RobotControl.get_laser(self.middle)
while (distance > 1):
self.RobotControl.move_straight()
distance = self.RobotControl.get_laser(self.middle)
self.RobotControl.stop_robot()
def checkBounds(self):
leftSide = self.RobotControl.get_laser(self.left)
rightSide = self.RobotControl.get_laser(self.right)
if (leftSide > rightSide):
self.turnDirection = 'clockwise'
self.RobotControl.turn(self.turnDirection, self.speed,
self.timeTurn)
else:
self.turnDirection = 'counter-clockwise'
self.RobotControl.turn(self.turnDirection, self.speed,
self.timeTurn)
class MoveRobot:
def __init__(self):
self.robot= RobotControl()
self.is_out_maze = False
def check_out_maze(self):
left = self.robot.get_laser(719)
right = self.robot.get_laser(0)
print ("Left side laser reading is %f" % left)
print ("Right side laser reading is %f" % right)
if str(left) == 'inf' and str(right) == 'inf':
self.is_out_maze = True
self.robot.stop_robot()
print ("Turtlebot is out of the maze!")
else:
print ("Turtlebot is not out of the maze yet!")
def direction_to_turn(self):
left = self.robot.get_laser(719)
right = self.robot.get_laser(0)
print ("Left side laser reading is %f" % left)
print ("Right side laser reading is %f" % right)
if left > right:
print ("Let's turn to the left!")
self.robot.turn('counterclockwise', 0.2, 7.7)
else:
print ("Let's turn to the right!")
self.robot.turn('clockwise', 0.2, 7.7)
def main(self):
while not self.is_out_maze:
front = self.robot.get_laser(360)
while front > 1.2 and not self.is_out_maze:
print ("Keep moving forward!")
self.robot.move_straight()
front = self.robot.get_laser(360)
self.robot.stop_robot()
print ("Wall is near... Stop!")
self.direction_to_turn()
self.check_out_maze()
from robot_control_class import RobotControl
rc = RobotControl()
a = rc.get_laser(360)
if a < 1:
rc.stop_robot()
else:
rc.move_straight()
print(a)
def sleept(t=5):
rc = RobotControl()
rc.move_straight()
time.sleep(t)
rc.stop_robot()
x = time.time()
robocontrol = RobotControl()
while True:
L, F, R = get_laser(1, 1)
print("L =", L, "F =", F, "R =", R)
if (L < 100 or F < 100 or R < 100):
if (((R < 0.6 or L < 0.6) and F > 1.3) or (L < 0.4 or R < 0.4)):
robocontrol.stop_robot()
if (R < 0.6):
print("go left_forward")
robocontrol.move_straight_time("backward", 0.3, 3)
robocontrol.turn("counter-clockwise", 0.3, 0.5)
robocontrol.move_straight()
elif (L < 0.6):
print("go right_forward")
robocontrol.move_straight_time("backward", 0.3, 3)
robocontrol.turn("clockwise", 0.3, 0.5)
robocontrol.move_straight()
elif F < 1.30:
robocontrol.stop_robot()
L, F, R = get_laser(2, 2)
if (F < 0.5):
print("go backward_slowly")
robocontrol.move_straight_time("backward", 0.3, 1)
elif (F < 0.90):
if (R > L):
print("Turn right")
robocontrol.turn("clockwise", 0.3, 5)
class MoveRobot:
def __init__(self, motion, clockwise, speed, time):
self.robot = RobotControl()
self.motion = motion
self.clockwise = clockwise
self.speed = speed
self.time = time
self.time_turn = 2.5
self.turn_speed = speed
self.full_laser = self.robot.get_laser_full()
self.full_laser2 = self.robot.get_laser_full()
#Code above initialize the construct and the variable class wich belongs the class MoveROBOT
def out_maze(self):
corner = 0
maximo = 0
#While loop used to find the labirint exit, by searching the door though the sensor "inf" value
#When the door is found then start the door_maze function below this one
#The full laser data is get setting the maximp< infinito, so it will always get the data
while(maximo < float("inf")):
self.full_laser = self.robot.get_laser_full()
maximo = 0
#The maximo was set to =0 above to enter in the while loop,but must be update in this loop to achieve a value
#bigger than 1 and so the robot contemplate a escape of a wall that it is close it. The code below is useful
#to let the robot get free from a "trap", in other words when it be very close to a wall
#The block below will turn the robot until if finds the higher distance value, while it is close the wall
while(maximo < 1):
#For loop to get the maximum value from the laser scan reading
for value in self.full_laser:
if value > maximo:
maximo = value
if(maximo < 1):
self.turn()
print("Current Maximum laser distace is %f" % maximo)
#After getting the maximum value you need to make the robot move in its direction.Turn the robot around
#till its frontal laser is the close enough to the maximum value - this is represented in the code by max-1
#and max!=infinite arguments. now its facing the right direction, or at least almost there.The codebelow
#will turn the robot around until the FRONT LASER BEAM = 360 find the max distance and get out the turn loop
#To be honest the loop would calculate forever the max value because it has 720 beams so each beam should be
#compared while turning to give back the biggest value. This is done faster using a tolerance,. This tolerance
#is put with maximo - [value] in this case 1 meter, so when the robot beams have a value of max aroun 6 meters
#for example it will fast accecpt the distance of 5 meter as the "infinite" distance and get out the turn loop
#to start the move_straight loop
while(self.robot.get_front_laser() < maximo -1 and maximo != float("inf")):
print("Robot frontal laser distance: %f", self.robot.get_front_laser())
self.turn()
#The code above is turning the robot until it finds the "first" distance shorter or close to the infinite
#distance value, when it arrives in this value (represented by max-1) it will get out the loop and stop
#turning. Then the code below enters in action ( to be honest is being compiled in paralell) so if the robot
#is not turning mean it is going ahead until find a wall (laser<1), then this programming block will stop_robot
# and the previous blocks will work (turn and turn the robot until find again an "almost-infinite" value)
#When one of these 3 blocks be reached the function stop_robot will work, avoiding simultaneously confusion
#while executing a move
while (self.robot.get_front_laser() > 1):
self.move_straight()
self.stop_robot()
def door_maze(self):
#Define the region frontal the robot to laser reading, 270 - 470 correspond 45 degrees of range which
#the robot will consider to calculate the max distace === infinit
#Here the laser scan data will calculate the most distance value = infinit. If not achieve the infinit
#value it means that it is near a wall and thus will go to the second loop (to turn and find the next
#laser scan data which is "infinite") the counter pass reading all the matrix laser data values, until
#get out the loop and then move_straight
#Her if the robot frontal (45 degrees cone range: 270:450) is already getting the infinite value, so the robot
#will go toward it. The counter +=1 is just to read all the values inside user laser ( the beams 270 to 450). If
#any of them calculate an infinite value the robot will go and try get out the maze
i_counter = 0
self.full_laser = self.robot.get_laser_full()
use_laser = self.full_laser[270:450]
for i in use_laser:
if i == float("inf"):
i_counter += 1
#Just enter in this loop if the door is not find at first
#Then the robot starts turn around until its facing the door
#The counter is used to define wheter the robot is in the rigth direction or not
#In case its not, then the robot will turn again till the criteria is met
#If the robot still did not find the infinite value (more distance from a wall), so it will need to turns
#because maybe it is turned with its back for the free destination. So the counter will read again the
#laser scan front robot range and turn and turn until find the infinite value (more distant from a obstacle)
#to get out the loop ( already read all the use_laser matrix values with counter and comapared to find the big one)
#and all the beams=90. So it can get out the loop, because found the greatest value and move straight out the maze!
while(i_counter < 90):
self.turn()
self.full_laser = self.robot.get_laser_full()
use_laser = self.full_laser[270:450]
i_counter = 0
for i in use_laser:
if i == float("inf"):
i_counter += 1
self.move_straight_time()
def move_straight_time(self):
self.robot.move_straight_time(self.motion, self.speed, self.time)
def move_straight(self):
self.robot.move_straight()
def turn(self):
self.robot.turn(self.clockwise,self.turn_speed,self.time_turn)
def stop_robot(self):
self.robot.stop_robot()
