Hello, I am working on an autonomous car powered by arduino mega and raspberry pi 4B 8GB. Arduino communicates with rpi via UART and rpi sends commands like FORWARD, LEFT etc. The arduino reads them and performs the movement according to the command received and also calculates the pwm from the speed measured by the ir encoders connected to the arduino and also sends the distance travelled to the rpi every 100ms. I also use a camera for a simple edge finding algorithm to detect obstacles and ground, then an ultrasonic sensor, the previously mentioned ir encoders on the motors and a compass. All the data is then used to create a simple 2D map (top view) of the searched area, which will in future gradually expand as the car uses some algorithm to find the best path. Now I'm stuck because I can't find a way to "run my script". Every time I run it, after about one second, rpi freezes but doesn't detect any error. I also made a "simulation" to test the mechanism on pc with some random values for the sensors and it works. Any ideas? (temperature on rpi is around 42°C)
Code:
# main.pyimport viewimport serial_busimport compassfrom motion import MotionControllerimport tcpdef main(): motion_controller = MotionController() while True: surrounding = view.check_ground() distance = view.ping() angle = compass.get_angle() mileage = serial_bus.read() direction = motion_controller.gen_map(surrounding, distance, angle, mileage) serial_bus.write(direction) if __name__ == "__main__": main()Code:
#compass.pyimport RPi.GPIO as GPIOimport smbusimport timeimport mathimport csvbus = smbus.SMBus(1)address = 0x1edef read_byte(adr): return bus.read_byte_data(address, adr)def read_word(adr): high = bus.read_byte_data(address, adr) low = bus.read_byte_data(address, adr+1) val = (high << 8) + low return valdef read_word_2c(adr): val = read_word(adr) if (val >= 0x8000): return -((65535 - val) + 1) else: return valdef write_byte(adr, value): bus.write_byte_data(address, adr, value) def get_angle(): write_byte(0, 0b01110000) write_byte(1, 0b00100000) write_byte(2, 0b00000000) scale = 0.92 x_offset = 38 y_offset = -129.5 x_out = (read_word_2c(3) - x_offset) * scale y_out = (read_word_2c(7) - y_offset) * scale z_out = (read_word_2c(5)) * scale bearing = math.atan2(y_out, x_out) if (bearing < 0): bearing += 2 * math.pi declination = 5.5 bearing = round((math.degrees(bearing) + declination), 1) #print("Bearing: ", bearing) return bearingCode:
#motion.pyimport tkinter as tkimport mathfrom enum import Enumcommands = ["STOP", "FRWD", "BKWD", "LEFT", "RIHT"]class Commands(Enum): STOP = 0 FRWD = 1 BKWD = 2 LEFT = 3 RIHT = 4class MotionController: def __init__(self): self.width, self.height = 1000, 1000 self.angle_line_startx = self.width / 2 self.angle_line_starty = self.height / 2 self.angle = 0 self.mileage = 0 self.line_length = 20 self.x_factor = self.width / 2 self.y_factor = self.height / 2 self.size_factor = 0.2 self.turn = False self.cmd_pick = 0 self.polygon_points = [] self.root = tk.Tk() self.root.title("Map") self.canvas = tk.Canvas(self.root, width=self.width, height=self.height, bg="black") self.canvas.pack() def arr_pos(self, x, y): image_width = 640 * self.size_factor image_height = 480 * self.size_factor x_rot_offset = self.x_factor - image_width / 2 y_rot_offset = self.y_factor - image_height / 2 angle_in_rad_rot = (self.angle + 90) * math.pi / 180 cos_theta = math.cos(angle_in_rad_rot) sin_theta = math.sin(angle_in_rad_rot) x_rot = self.size_factor * (x - self.x_factor) * cos_theta - self.size_factor * (y - self.y_factor) * sin_theta + x_rot_offset y_rot = self.size_factor * (x - self.x_factor) * sin_theta + self.size_factor * (y - self.y_factor) * cos_theta + y_rot_offset angle_in_rad_transform = self.angle * math.pi / 180 cos_fi = math.cos(angle_in_rad_transform) sin_fi = math.sin(angle_in_rad_transform) x_transform_offset = self.mileage * self.size_factor * cos_fi y_transform_offset = self.mileage * self.size_factor * sin_fi x_transformed = x_rot + x_transform_offset y_transformed = y_rot + y_transform_offset return x_transformed, y_transformed def gen_map(self, surrounding, distance, angle, mileage): self.angle = angle self.mileage = mileage angle_in_rad = angle * math.pi / 180 cos_theta = math.cos(angle_in_rad) sin_theta = math.sin(angle_in_rad) connection_points = [(0, 0),(0, 0)] angle_line_endx = self.angle_line_startx + self.line_length * cos_theta angle_line_endy = self.angle_line_starty + self.line_length * sin_theta self.canvas.delete("angle_line") self.canvas.create_line(self.angle_line_startx, self.angle_line_starty, angle_line_endx, angle_line_endy, fill="red", tags="angle_line") if mileage % 500 == 0: #just random val to see if mapping works self.turn == True if self.turn == True: x_start_rot, y_start_rot = self.arr_pos(surrounding[0][0], surrounding[0][1]) x_end_rot, y_end_rot = self.arr_pos(surrounding[len(surrounding)-2][0], surrounding[len(surrounding)-2][1]) self.x_factor = (x_start_rot + x_end_rot) / 2 self.y_factor = (y_start_rot + y_end_rot) / 2 self.turn = False self.polygon_points = [self.arr_pos(x, y) for x, y in surrounding] self.canvas.create_polygon(self.polygon_points, outline="white", fill="white", tags="map_polygon") if distance < 30: self.cmd_pick = Commands.STOP.value else: if(angle >= angle + 90): self.cmd_pick = Commands.RIHT.value else: self.cmd_pick = Commands.FRWD.value direction = commands[self.cmd_pick] self.root.update() return directionCode:
#serial_bus.pyimport serialimport time ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)ser.reset_input_buffer()mileage = 0def write(direction): ser.write((direction + '\n').encode()) def read(): global mileage if ser.inWaiting() > 0: mileage = int(ser.readline().decode('UTF-8').strip()) return mileageCode:
#view.pyimport osos.environ['QT_QPA_PLATFORM'] = 'xcb'import cv2import numpy as npimport RPi.GPIO as GPIOimport schedimport timecapture = cv2.VideoCapture(0)capture.set(cv2.CAP_PROP_FRAME_WIDTH, 640)capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)last_distance = 0def check_ground(): step_size = 6 fov_width = 173 edge_array = [] _, img = capture.read() img = cv2.resize(img, (640, 480)) img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) img_gray = cv2.bilateralFilter(img_gray, 10, 25, 50) kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10,10)) dilate = cv2.morphologyEx(img_gray, cv2.MORPH_DILATE, kernel) img_edge = cv2.Canny(dilate, 35, 150) imagewidth = img_edge.shape[1] - 1 imageheight = img_edge.shape[0] - 1 for j in range(0, imagewidth, step_size): for i in range(imageheight-5, 0, -1): if img_edge.item(i, j) == 255: edge_array.append((j, i)) break else: edge_array.append((j, 0)) edge_array.insert(0, (0, imageheight)) edge_array.append((imagewidth, imageheight)) top_left = (0, 0) bottom_left = (0, imageheight) top_right = (imagewidth, 0) bottom_right = (imagewidth, imageheight) pts1 = np.float32([top_left, bottom_left, top_right, bottom_right]) pts2 = np.float32([[fov_width, 0], [0, imageheight], [(imagewidth - fov_width), 0], [imagewidth, imageheight]]) transform_matrix = cv2.getPerspectiveTransform(pts1, pts2) transformed_edge_array = [] for point in edge_array: x, y = point original_coordinates = np.array([x, y, 1]) transformed_coordinates = np.dot(transform_matrix, original_coordinates) transformed_x = int(transformed_coordinates[0] / transformed_coordinates[2]) transformed_y = int(transformed_coordinates[1] / transformed_coordinates[2]) transformed_edge_array.append((transformed_x, transformed_y)) cv2.waitKey(1) return transformed_edge_arraydef ping(): global last_distance GPIO.setmode(GPIO.BOARD) TRIG_PIN = 13 ECHO_PIN = 15 GPIO.setup(TRIG_PIN, GPIO.OUT) GPIO.setup(ECHO_PIN, GPIO.IN) pulse_start_time = 0 pulse_end_time = 0 try: GPIO.output(TRIG_PIN, GPIO.LOW) time.sleep(2E-6) GPIO.output(TRIG_PIN, GPIO.HIGH) time.sleep(10E-6) GPIO.output(TRIG_PIN, GPIO.LOW) while GPIO.input(ECHO_PIN) == 0: pulse_start_time = time.time() while GPIO.input(ECHO_PIN) == 1: pulse_end_time = time.time() pulse_duration = pulse_end_time - pulse_start_time distance = round(pulse_duration * 17150, 2) if distance > 300 or distance <= 0: distance = last_distance else: last_distance = distance #print(distance) time.sleep(0.1) return distance except Exception as e: print(f"Error in ping function: {e}")Statistics: Posted by mart1nek — Mon Jan 29, 2024 7:36 pm
