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 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()
class square: def __init__(self, motion, clockwise, speed, time): self.rc = RobotControl() self.motion = motion self.clockwise = clockwise self.speed = speed self.time = time self.turn_time = 7 def sq(self): i = 0 while i < 4: self.move_straight() self.turn() i += 1 def move_straight(self): self.rc.move_straight_time(self.motion, self.speed, self.time) def turn(self): self.rc.turn(self.clockwise, self.speed, self.turn_time)
class MoveRobot: def __init__(self, motion, clockwise, speed, time): self.RC = RobotControl() self.motion = motion self.clockwise = clockwise self.speed = speed self.time = time self.time_turn = 7.0 def move_straight(self): self.RC.move_straight_time(self.motion, self.speed, self.time_turn) def turn(self): self.RC.turn(self.clockwise, self.speed, self.time_turn) def do_square(self): i = 0 while (i < 4): self.move_straight() self.turn() i = i + 1
class MoveRobot: def __init__(self, motion, clockwise, speed, time): self.robotcontrol = RobotControl(robot_name="summit") self.motion = motion self.clockwise = clockwise self.speed = speed self.time = time self.time_turn = 7.0 # This is an estimate time in which the robot will rotate 90 degrees def do_square(self): i = 0 while (i < 4): self.move_straight() self.turn() i+=1 def move_straight(self): self.robotcontrol.move_straight_time(self.motion, self.speed, self.time) def turn(self): self.robotcontrol.turn(self.clockwise, self.speed, self.time_turn)
from robot_control_class import RobotControl rc = RobotControl() rc.stop_robot() rc.move_straight_time("forward", 4, 0.15) rc.turn("counter-clockwise", 4, .5) rc.move_straight_time("forward", 4, .5) rc.turn("counter-clockwise", 4, .5) rc.move_straight_time("forward", 10, .2)
from robot_control_class import RobotControl robot = RobotControl() def movimento(): global a global b a = "forward" b = "counter-clockwise" movimento() direcao = robot.move_straight_time(a, 0.75, 5) giro = robot.turn(b, 0.45, 10) print ("O robo se deslocou com os seguinte parametros:", direcao ) print ("O robo girou com os seguintes parametros:", giro )
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()
return L2, F, R2 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")
# Demonstrate two new RobotControl functs # Req: Move robot forward 5s. Turn robot clockwise 7s # Print funct output after each movement from robot_control_class import RobotControl rc = RobotControl() # Move robot straight at 1 m/s for 5 s # Args: direction, speed m/s, time s print rc.move_straight_time('forward', 1, 5) # Rotate robot clockwise at 2 m/s for 7 s # Args: direction, speed m/s, time s print rc.turn('clockwise', 2, 7)
from robot_control_class import RobotControl rc = RobotControl(robot_name="summit") rc.move_straight_time("forward", 0.2, 5) rc.turn("counter-clockwise", 0.2, 13) rc.move_straight_time("forward", 0.2, 10) rc.turn("counter-clockwise", 0.2, 11) rc.move_straight_time("forward", 0.2, 10) print("Bellos is my lord and savior")
from robot_control_class import RobotControl robotcontrol = RobotControl() robotcontrol.turn("counter-clockwise", 0.3, 4) robotcontrol.move_straight_time("forward", 0.3, 6) robotcontrol.turn("counter-clockwise", 0.3, 4) robotcontrol.move_straight_time("forward", 0.3, 7)
from robot_control_class import RobotControl rc = RobotControl() rc.stop_robot() while True: d = rc.get_laser(360) print(d) if (d > 1): rc.move_straight() else: rc.stop_robot() l = rc.get_laser(719) r = rc.get_laser(1) if (l > r): rc.turn("counter-clockwise",4,.399) else: rc.turn("clockwise",4,.399)
from robot_control_class import RobotControl import time import math rc=RobotControl() print(rc.move_straight_time("forward",1,1)) print(rc.turn("counter-clockwise",2,1)) print(rc.move_straight_time("forward",1,2)) print(rc.turn("counter-clockwise",2,1)) print(rc.move_straight_time("forward",1,2))
from robot_control_class import RobotControl import math rc = RobotControl() laser = rc.get_laser_full() while laser[360] >= 1: rc.move_straight() laser = rc.get_laser_full() rc.stop_robot() rc.turn("clockwise", 0.2, 7)
from robot_control_class import RobotControl rc = RobotControl() while rc.get_laser(360) > 1: rc.move_straight() rc.stop_robot() rc.rotate(80) rc.stop_robot() while rc.get_laser(360) > 1: rc.move_straight() rc.stop_robot() rc.rotate(80) rc.stop_robot() while rc.get_laser(360) > 1: rc.move_straight() rc.stop_robot() rc.rotate(-90) rc.stop_robot() print(rc.get_laser_full()) rc.move_straight_time("forward", 0.5, 7) rc.turn("clockwise", 0.2, 2) rc.move_straight_time("forward", 0.5, 7)
from robot_control_class import RobotControl rc = RobotControl() #exercise 4.3 #rc.move_straight_time("forward", 1.0, 5) #rc.turn("clockwise", 190, 7) #exercise 4.4 rc.move_straight_time("forward", 1.0, 2) rc.turn("clockwise", 1.0, 7) rc.move_straight_time("forward", 1.0, 2.2) rc.turn("clockwise", 1.0, 7) rc.move_straight_time("forward", 1.0, 5)
from robot_control_class import RobotControl import time as t rc = RobotControl() rc.stop_robot() while True: d = rc.get_laser(360) f = rc.get_laser(0) if (d < 1.0): #Stop Robot rc.stop_robot() rc.turn("clockwise", 0.3, 5.25) if (d < 1.0 and f < 1.3): rc.stop_robot() rc.turn("counterclockwise", 0.6, 5.35) else: #Move Forward rc.move_straight() print("distance1 = ", d) print("distance2 = ", f)
# Move Robot into room with orange bench # Req: Utilize RobotControl functs discussed from robot_control_class import RobotControl rc = RobotControl() # Move robot into the room with Orange bench # Args: direction, speed m/s, time s rc.move_straight_time('forward', 2.5, 0.5) rc.turn('counter-clockwise', 2, 1.07) rc.move_straight_time('forward', 2.5, 0.7) rc.turn('counter-clockwise', 2, 1.07) rc.move_straight_time('forward', 2.5, 1.6) rc.turn('clockwise', 2, 2.14)
from robot_control_class import RobotControl rc = RobotControl(robot_name="summit") rc.move_straight_time("forward", 2, 5) rc.turn("clockwise", 2, 7)