class Car: def __init__(self, sensors, wheels, carBody, clientID): #print("dsdsdsd") self.target_velocity = 2 self.stop_velocity = 0 self.turning_speed = 0.5 self.turning_time = 1.80 self.rotation_force = 100 #self.map = Map(13, 13, [12, 8])#old labirynth01 self.map = Map(20, 20, [18, 1])#new labirynth02 #self.clientID = 0 self.sensors = sensors self.wheels = wheels self.clientID = clientID self.carBody = carBody self.cell_size = 1 self.cell_size_half = 0.5 self.travel_speed = 10 self.travel_max_speed = 30 self.rotation_speed = 5 self.rotation_error = 0.5 self.frontSensor = self.sensors[0] self.leftSensor = self.sensors[5] self.rightSensor = self.sensors[4] self.direction = self.get_car_direction() def random_test(self): i = 0 while i < 4: self.rotate_right() print(str(self.getCarHorizontalAngle()) + " > " + str(self.get_car_direction())) i += 1 time.sleep(1) i = 0 while i < 4: self.rotate_left() print(str(self.getCarHorizontalAngle()) + " > " + str(self.get_car_direction())) i += 1 time.sleep(1) #print("======================") #time.sleep(3) def sensor_test(self): frontSensor = self.sensors[0] leftSensor = self.sensors[5] rightSensor = self.sensors[4] while True: print("right = " + str(self.sensorDistance(rightSensor))) print("left = " + str(self.sensorDistance(leftSensor))) print("front = " + str(self.sensorDistance(frontSensor))) time.sleep(1) def is_correct(self, measured_distance): if measured_distance > 0 : return True return False def mapping_run(self): self.get_car_direction() self.map.set_values(self.direction, self.sensorDistance(self.leftSensor), self.sensorDistance(self.rightSensor), self.sensorDistance(self.frontSensor)) while True: print("===================================== next iteration:") self.get_car_direction()#should not be necesary, bc car should not rotate before if self.map.is_all_discovered(): print("map discovered") break forward = self.map.check(self.direction, Direction.FORWARD) left = self.map.check(self.direction, Direction.LEFT) right = self.map.check(self.direction, Direction.RIGHT) print("info about surroundings:") print("left forward right") print(str(left) + " " + str(forward) + " " + str(right)) print("decide where to go ") direction_to_go = self.choose_direction(left, forward, right) if direction_to_go == Direction.FORWARD: print("going forward") elif direction_to_go == Direction.BACKWARD: print("going back") self.rotate_180() elif direction_to_go == Direction.LEFT: print("going left") self.rotate_left() elif direction_to_go == Direction.RIGHT: print("going right") self.rotate_right() #go self.goForwardTillSensorDetectMap(self.travel_speed, 0.8, self.direction)# will travel by one cell size and also update map after travel def after_mapping(self): self.rotate_180() self.ride_from_point_to_point(self.map.currentPosition, self.map.startPosition) self.ride_from_point_to_point(self.map.startPosition, self.map.endPosition) def choose_direction(self, left, forward, right): direction_to_go = None print("brrr, looking for path to nearest clear point on map ... ") path_to_clear = self.map.BFSpathToClear() print("found path :") for direction in path_to_clear: print(Direction.get_direction_from_vector(direction)) if path_to_clear[0] == Direction.NORTH.value:# y == 1:#should go north if self.direction == Direction.NORTH.value: return Direction.FORWARD if self.direction == Direction.SOUTH.value: return Direction.BACKWARD if self.direction == Direction.WEST.value: return Direction.RIGHT if self.direction == Direction.EAST.value: return Direction.LEFT if path_to_clear[0] == Direction.SOUTH.value:#y == 0:#should go south if self.direction == Direction.NORTH.value: return Direction.BACKWARD if self.direction == Direction.SOUTH.value: return Direction.FORWARD if self.direction == Direction.WEST.value: return Direction.LEFT if self.direction == Direction.EAST.value: return Direction.RIGHT if path_to_clear[0] == Direction.WEST.value:#x == 1:#should go west if self.direction == Direction.NORTH.value: return Direction.LEFT if self.direction == Direction.SOUTH.value: return Direction.RIGHT if self.direction == Direction.WEST.value: return Direction.FORWARD if self.direction == Direction.EAST.value: return Direction.BACKWARD if path_to_clear[0] == Direction.EAST.value:#x == 0:#should go east if self.direction == Direction.NORTH.value: return Direction.RIGHT if self.direction == Direction.SOUTH.value: return Direction.LEFT if self.direction == Direction.WEST.value: return Direction.BACKWARD if self.direction == Direction.EAST.value: return Direction.FORWARD def rotate_left(self): current_car_direction = self.get_car_direction() if current_car_direction == Direction.NORTH: self.rotate_car_to_direction(Direction.WEST) self.direction = Direction.WEST.value if current_car_direction == Direction.WEST: self.rotate_car_to_direction(Direction.SOUTH) self.direction = Direction.SOUTH.value if current_car_direction == Direction.SOUTH: self.rotate_car_to_direction(Direction.EAST) self.direction = Direction.EAST.value if current_car_direction == Direction.EAST: self.rotate_car_to_direction(Direction.NORTH) self.direction = Direction.NORTH.value def rotate_right(self): current_car_direction = self.get_car_direction() if current_car_direction == Direction.NORTH: self.rotate_car_to_direction(Direction.EAST) self.direction = Direction.EAST.value if current_car_direction == Direction.WEST: self.rotate_car_to_direction(Direction.NORTH) self.direction = Direction.NORTH.value if current_car_direction == Direction.SOUTH: self.rotate_car_to_direction(Direction.WEST) self.direction = Direction.WEST.value if current_car_direction == Direction.EAST: self.rotate_car_to_direction(Direction.SOUTH) self.direction = Direction.SOUTH.value def rotate_180(self): current_car_direction = self.get_car_direction() self.rotate_right() self.rotate_right() if current_car_direction == Direction.NORTH: #self.rotate_car_to_direction(Direction.SOUTH) self.direction = Direction.SOUTH.value if current_car_direction == Direction.WEST: #self.rotate_car_to_direction(Direction.EAST) self.direction = Direction.EAST.value if current_car_direction == Direction.SOUTH: #self.rotate_car_to_direction(Direction.NORTH) self.direction = Direction.NORTH.value if current_car_direction == Direction.EAST: #self.rotate_car_to_direction(Direction.WEST) self.direction = Direction.WEST.value def rotate_car_to_direction(self, direction): if direction == Direction.NORTH: self.rotateCarToDeg(0, self.rotation_speed, self.rotation_error) self.direction = Direction.NORTH.value if direction == Direction.WEST: self.rotateCarToDeg(90, self.rotation_speed, self.rotation_error) self.direction = Direction.WEST.value if direction == Direction.SOUTH: self.rotateCarToDeg(180, self.rotation_speed, self.rotation_error) self.direction = Direction.SOUTH.value if direction == Direction.EAST: self.rotateCarToDeg(-90, self.rotation_speed, self.rotation_error) self.direction = Direction.EAST.value def get_car_direction(self): angle = self.getCarHorizontalAngle() self.direction = Direction.get_direction_from_angle(angle).value return Direction.get_direction_from_angle(angle) def run(self): #go forward till front sensor close frontSensor = self.sensors[0] leftSensor = self.sensors[5] rightSensor = self.sensors[4] #frontSensor = sensors[2] while True: self.goForwardTillSensorDetect(5, frontSensor, 0.2) print("rotate till sensor front clear") print("frontSensor = " + str(self.sensorDistance(frontSensor))) print("rightSensor = " + str(self.sensorDistance(rightSensor))) print("leftSensor = " + str(self.sensorDistance(leftSensor))) #rotate left if right sensor detects if self.sensorDistance(rightSensor) < 1 : while self.sensorDistance(frontSensor) < 0.5 and self.sensorDistance(frontSensor) > 0.001: self.rotateCarByDeg(-10, 2) else: #$self.sensorDistance(leftSensor) < 1 : while self.sensorDistance(frontSensor) < 0.5 and self.sensorDistance(frontSensor) > 0.001: self.rotateCarByDeg(10, 2) #rotate right if left sensor detects def square(self): while(True): self.rotateCarToDeg(0, self.turning_speed, 0.1) time.sleep(1) self.goForward(5, 1, 0.001) time.sleep(1) self.rotateCarToDeg(-90, self.turning_speed, 0.1) time.sleep(1) self.goForward(5, 1, 0.001) time.sleep(1) self.rotateCarToDeg(-180, self.turning_speed, 0.1) time.sleep(1) self.goForward(5, 1, 0.001) time.sleep(1) self.rotateCarToDeg(90, self.turning_speed, 0.1) time.sleep(1) self.goForward(5, 1, 0.001) time.sleep(1) # direction: previous direction, rotation: 1 - right, -1 - left, 0 - no rotation def set_direction(self, direction, rotation): if direction[0] == -1 and direction[1] == 0: if rotation == 1: direction = [0, 1] elif rotation == -1: direction = [0, -1] elif direction[0] == 1 and direction[1] == 0: if rotation == 1: direction = [0, -1] elif rotation == -1: direction = [0, 1] elif direction[0] == 0 and direction[1] == -1: if rotation == 1: direction = [-1, 0] elif rotation == -1: direction = [1, 0] elif direction[0] == 0 and direction[1] == 1: if rotation == 1: direction = [1, 0] elif rotation == -1: direction = [-1, 0] return direction def sensorDistance(self, sensor): return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(self.clientID, sensor, sim.simx_opmode_blocking) #print("detectionState = " + str(detectionState)) if detectionState == False: distance = -1 else: distance = math.sqrt(detectionPoint[0]*detectionPoint[0] + detectionPoint[1]*detectionPoint[1] + detectionPoint[2]*detectionPoint[2]) return distance def rotate(self): sim.simxSetJointTargetVelocity(self.clientID, self.wheels[0], self.turning_speed, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[2], turning_speed, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[1], -self.turning_speed, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[3], -self.turning_speed, sim.simx_opmode_oneshot) time.sleep(self.turning_time) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[0], self.stop_velocity, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[2], self.stop_velocity, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[1], self.stop_velocity, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[3], self.stop_velocity, sim.simx_opmode_oneshot) def set_wheels_force(self, left, right): sim.simxSetJointForce(self.clientID, self.wheels[0], left, sim.simx_opmode_oneshot) sim.simxSetJointForce(self.clientID, self.wheels[2], left, sim.simx_opmode_oneshot) sim.simxSetJointForce(self.clientID, self.wheels[1], right, sim.simx_opmode_oneshot) sim.simxSetJointForce(self.clientID, self.wheels[3], right, sim.simx_opmode_oneshot) def set_wheels_velocity(self, left, right): sim.simxSetJointTargetVelocity(self.clientID, self.wheels[0], left, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[2], left, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[1], right, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(self.clientID, self.wheels[3], right, sim.simx_opmode_oneshot) def getCarHorizontalAngle(self): return_value, eulerAngles = sim.simxGetObjectOrientation(self.clientID, self.carBody, -1, sim.simx_opmode_oneshot) angle = eulerAngles[2] * 180 / math.pi #self.detect_object_quick() return angle def rotateCarByDegMap(self, degrees, speed): if degrees == 0: return if degrees < 0: print("rotate left") speed = -speed degrees = abs(degrees) else: print("rotate right") starting_angle = self.getCarHorizontalAngle() print(starting_angle) #start rotating self.set_wheels_velocity(speed, -speed) #print("starting to rotate ...") while(True): #print("rotated: " + str(abs(starting_angle - self.getCarHorizontalAngle()))) if (abs(starting_angle - self.getCarHorizontalAngle())) >= degrees : self.set_wheels_velocity(0, 0) break if (abs(starting_angle - self.getCarHorizontalAngle())) >= (degrees-2) : self.set_wheels_velocity(speed/8, -speed/8) continue if (abs(starting_angle - self.getCarHorizontalAngle())) >= (degrees-5) : self.set_wheels_velocity(speed/4, -speed/4) continue #correct 1 if (abs(starting_angle - self.getCarHorizontalAngle())) > degrees : #print("correcting ...") self.set_wheels_velocity(-speed/16, speed/16) while(True): #print("rotated: " + str(starting_angle - self.getCarHorizontalAngle())) if (abs(starting_angle - self.getCarHorizontalAngle())) <= degrees : self.set_wheels_velocity(0, 0) break #correct 2 if (abs(starting_angle - self.getCarHorizontalAngle())) < degrees : #print("correcting ...") self.set_wheels_velocity(speed/32, -speed/32) while(True): #print("rotated: " + str(starting_angle - getCarHorizontalAngle())) if (abs(starting_angle - self.getCarHorizontalAngle())) >= degrees : self.set_wheels_velocity(0, 0) break return abs(starting_angle - self.getCarHorizontalAngle())-degrees def rotateCarByDeg(self, degrees, speed): if degrees == 0: return if degrees < 0: #print("rotate left") speed = -speed degrees = abs(degrees) else: pass #print("rotate right") starting_angle = self.getCarHorizontalAngle() #print(starting_angle) #start rotating self.set_wheels_velocity(speed, -speed) #print("starting to rotate ...") while(True): #print("rotated: " + str(abs(starting_angle - self.getCarHorizontalAngle()))) if (abs(starting_angle - self.getCarHorizontalAngle())) >= degrees : self.set_wheels_velocity(0, 0) break if (abs(starting_angle - self.getCarHorizontalAngle())) >= (degrees-2) : self.set_wheels_velocity(speed/8, -speed/8) continue if (abs(starting_angle - self.getCarHorizontalAngle())) >= (degrees-5) : self.set_wheels_velocity(speed/4, -speed/4) continue #correct 1 if (abs(starting_angle - self.getCarHorizontalAngle())) > degrees : #print("correcting ...") self.set_wheels_velocity(-speed/16, speed/16) while(True): #print("rotated: " + str(starting_angle - self.getCarHorizontalAngle())) if (abs(starting_angle - self.getCarHorizontalAngle())) <= degrees : self.set_wheels_velocity(0, 0) break #correct 2 if (abs(starting_angle - self.getCarHorizontalAngle())) < degrees : #print("correcting ...") self.set_wheels_velocity(speed/32, -speed/32) while(True): #print("rotated: " + str(starting_angle - getCarHorizontalAngle())) if (abs(starting_angle - self.getCarHorizontalAngle())) >= degrees : self.set_wheels_velocity(0, 0) break #time.sleep(0.050) #print("rotated car from {0}, to {1} deg, error={2}".format(starting_angle, self.getCarHorizontalAngle(), abs(starting_angle - self.getCarHorizontalAngle())-degrees )) def rotateCarToDeg(self, degree, speed, error):#TODO write from scratch xd print("rotateCarToDeg: rotating from {0}, to {1} ...".format(self.getCarHorizontalAngle(), degree)) starting_angle = self.getCarHorizontalAngle() print(" brrr rotate start, from: " + str(starting_angle) + ", to degree: " + str(degree)) if starting_angle < 0 and degree == 180: degree = -180 diff = degree - starting_angle #if diff > 180: # diff -= 180 rotateLeft = True # + left # - right if diff > 0: speed = -speed #degrees = abs(degrees) else: rotateLeft = False if ((degree < 0 and starting_angle > 0) or (degree > 0 and starting_angle < 0)) and (abs(starting_angle) >= 90 and abs(degree) >= 90): speed = -speed rotateLeft = not rotateLeft if rotateLeft: print("rotateCarToDeg: rotate left") else: print("rotateCarToDeg: rotate right") #start rotating self.set_wheels_velocity(speed, -speed) #print("starting to rotate ...") i = 0 while(True): #self.detect_object(self.sensors[0], 1) difference = degree - self.getCarHorizontalAngle() #if i % 1000 == 0: #print("{1} => raw difference = {0}".format(difference, i)) if difference > 180: difference -= 360 if difference < -180: difference += 360 #if i % 1000 == 0: #print("{1} => clean difference = {0}".format(difference, i)) i += 1 if rotateLeft: if difference <= error : self.set_wheels_velocity(0, 0) print(" brrr rotate finished, difference <= error: " + str(difference) + " <= " + str(error)) break # if difference <= 1: # self.set_wheels_velocity(speed/16, -speed/16) # continue if difference <= 5: self.set_wheels_velocity(speed/8, -speed/8) continue if difference <= 10 : self.set_wheels_velocity(speed/4, -speed/4) continue else: if difference >= -error : self.set_wheels_velocity(0, 0) print(" brrr rotate finished, difference >= -error: " + str(difference) + " >= " + str(-error)) break # if difference >= -1: # self.set_wheels_velocity(speed/16, -speed/16) # continue if difference >= -5: self.set_wheels_velocity(speed/8, -speed/8) continue if difference >= -10 : self.set_wheels_velocity(speed/4, -speed/4) continue #print("rotateCarToDeg: rotated car from {0}, to {1}, target={2}".format(starting_angle, self.getCarHorizontalAngle(), degree )) def getCarPosition(self): return_value, pos = sim.simxGetObjectPosition(self.clientID, self.carBody, -1, sim.simx_opmode_oneshot) #self.detect_object_quick() return pos def goForwardTillSensorDetect(self, speed, sensor, walldistance): while(True): distance = self.sensorDistance(sensor) if distance < walldistance and distance > 0.001 : print("detected wall") break else: self.set_wheels_velocity(speed, speed) self.set_wheels_velocity(0, 0) def goForwardTillSensorDetectMap(self, speed, walldistance, direction): i = 0 while(True): self.get_car_direction() self.map.set_values(self.direction, self.sensorDistance(self.leftSensor), self.sensorDistance(self.rightSensor), self.sensorDistance(self.frontSensor)) error = self.goForwardMap(speed, self.cell_size, 0.01)# car actually travel after 2nd time, dunno why xd if abs(error) < 0.15:# if actually moved leftSensorDistance = self.sensorDistance(self.leftSensor) rightSensorDistance = self.sensorDistance(self.rightSensor) frontDistance = self.sensorDistance(self.frontSensor) #print("Go Forward") self.get_car_direction() self.map.update_position(self.direction) self.map.set_values(self.direction, leftSensorDistance, rightSensorDistance, frontDistance) break else: i += 1 print("wrong move, trying again ..." + str(error)) if i >= 100: i = 0 print("ERROR ERROR ERROR ERROR ") self.set_wheels_velocity(0, 0) return direction def goForwardMap(self, speed, distance_to_travel, error): startingPosition = self.getCarPosition() print(" brrr travel start, from: " + str(startingPosition)) self.set_wheels_velocity(speed, speed) #print("starting to move with speed {0}".format(speed)) while(True): wall_distance = self.sensorDistance(self.frontSensor) newPosition = self.getCarPosition() diff = [startingPosition[0] - newPosition[0], startingPosition[1] - newPosition[1]] traveled_distance = math.sqrt(diff[0]*diff[0] + diff[1]*diff[1]) if self.is_correct(wall_distance) and wall_distance < (self.cell_size_half-0.1):# if too close to wall, stop self.set_wheels_velocity(0, 0) #print("ABORT MOVEMENT, WALL DETECTED") #self.rotate_180() #self.goForwardMap(speed, traveled_distance, error) return traveled_distance - distance_to_travel #print(" ... startingPosition= {0}, new = {1}, diff = {2}, d = {3}".format(startingPosition, newPosition, diff, traveled_distance)) #print(" ... distance_to_travel - traveled_distance = " + str(distance_to_travel - traveled_distance)) if distance_to_travel - traveled_distance < error : self.set_wheels_velocity(0, 0) #print("stopped, error = {0}".format(traveled_distance - distance_to_travel)) print(" brrr travel end, traveled: " + str(traveled_distance)) return traveled_distance - distance_to_travel if distance_to_travel - traveled_distance < 0.01 : self.set_wheels_velocity(speed/4, speed/4) continue if distance_to_travel - traveled_distance < 0.1 : self.set_wheels_velocity(speed/2, speed/2) continue def goForward(self, speed, distance, error): startingPosition = self.getCarPosition() self.set_wheels_velocity(speed, speed) #print("starting to move with speed {0}".format(speed)) while(True): newPosition = self.getCarPosition() diff = [startingPosition[0] - newPosition[0], startingPosition[1] - newPosition[1]] d = math.sqrt(diff[0]*diff[0] + diff[1]*diff[1]) #print("startingPosition= {0}, new = {1}, diff = {2}, d = {3}".format(startingPosition, newPosition, diff, d)) if distance - d < error : self.set_wheels_velocity(0, 0) print("stopped, error = {0}".format(d - distance)) break if distance - d < 0.01 : self.set_wheels_velocity(speed/4, speed/4) continue if distance - d < 0.1 : self.set_wheels_velocity(speed/2, speed/2) continue def wheels_pos(self): return sim.simxGetJointPosition(self.clientID, self.wheels[0], sim.simx_opmode_oneshot)[1], sim.simxGetJointPosition(self.clientID, self.wheels[1], sim.simx_opmode_oneshot)[1], sim.simxGetJointPosition(self.clientID, self.wheels[2], sim.simx_opmode_oneshot)[1], sim.simxGetJointPosition(self.clientID, self.wheels[3], sim.simx_opmode_oneshot)[1] def detect_object(self, sensorHolder, miliseconds): x = 0 while(x<miliseconds): return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(self.clientID, self.sensors[0], sim.simx_opmode_buffer) if(detectionState==True and x%100==0): print(detectionPoint) print(detectionState) #sim.simxAddStatusbarMessage(clientID,'Detected something',sim.simx_opmode_oneshot) time.sleep(0.001) x+=1 def detect_object_quick(self): return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(self.clientID, self.sensors[0], sim.simx_opmode_buffer) if(detectionState==True): print(detectionPoint) print(detectionState) sim.simxAddStatusbarMessage(self.clientID,'Detected something',sim.simx_opmode_oneshot) def ride_from_point_to_point(self, startPoint, endPoint): self.map.path = self.map.astar(startPoint, endPoint) self.map.create_directions_from_path(self.get_car_direction(), endPoint) print(startPoint) print(endPoint) self.map.ridingFromPointToPoint = True while True: print("===================================== next iteration:") #self.get_car_direction()#should not be necesary, bc car should not rotate before #print(" decide where to go ") direction_to_go = self.map.directions[self.map.currentPathIndex] if self.map.check_if_end(endPoint, 0): self.map.currentPosition = endPoint print("Arrived at destination") break if direction_to_go == Direction.FORWARD: print("going forward") elif direction_to_go == Direction.BACKWARD: print("going back") self.rotate_180() elif direction_to_go == Direction.LEFT: print("going left") self.rotate_left() elif direction_to_go == Direction.RIGHT: print("going right") self.rotate_right() distance = self.calculate_distance_from_directions(endPoint) print("Move places forward : " + str(distance)) speed = self.travel_speed if distance > 1: speed = self.travel_speed + 5*distance if speed > self.travel_max_speed: speed = 30 #go self.goForwardRunFromPointToPoint(speed, 0.8, self.direction, distance)# will travel by one cell size and also update map after travel self.map.currentPathIndex = self.map.currentPathIndex + distance self.map.currentPosition = self.map.path[self.map.currentPathIndex-1] #self.rotate_180() def calculate_distance_from_directions(self, endPoint): distance = 1 offset = 1 while True: if self.map.check_if_end(endPoint, offset): print("Arrived at destination") break if self.map.directions[self.map.currentPathIndex + offset] == Direction.FORWARD: distance = distance + 1 offset = offset + 1 else: break return distance def goForwardRunFromPointToPoint(self, speed, walldistance, direction, distance): print("Traveling speed : " + str(speed)) i = 0 while(True): self.get_car_direction() error = self.goForwardMap(speed, distance, 0.01)# car actually travel after 2nd time, dunno why xd if abs(error) < 0.15:# if actually moved self.get_car_direction() self.map.update_position(self.direction) break else: i += 1 print("wrong move, trying again ..." + str(error)) if i >= 100: i = 0 print("ERROR ERROR ERROR ERROR ") self.set_wheels_velocity(0, 0) return direction def test_ride(self): self.map.set_map_for_testing() self.ride_from_point_to_point(self.map.startPosition, [7, 2]) self.ride_from_point_to_point([7, 2], self.map.startPosition)