Lidar POC for Vincent

Last Updated or created 2022-04-01

Using Python, Raspberry, a lidar module, a servo, display .. and a 3Dprinted holder.

Proof of concept was to see if it was easy to implement a lidar radar for boat navigation.

#!/usr/bin/env python
import math
import random
import pygame
from pygame.locals import *
import time
import serial
import pigpio

GPIO = 18
ser = serial.Serial("/dev/ttyUSB0", 115200)

pi = pigpio.pi()
distance = 0
pygame.init()
screen = pygame.display.set_mode((480, 320))
while True:
        screen.fill((0,0,0))
        pygame.draw.circle(screen, (0,128,0), (240,320), 100,1)
        pygame.draw.circle(screen, (0,128,0), (240,320), 200,1)
        pygame.draw.circle(screen, (0,128,0), (240,320), 300,1)
        for deze in range(10,170,5):
            count = ser.in_waiting
            if count > 8:
                    recv = ser.read(9)
                    ser.reset_input_buffer()
                    if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y'
                        low = int(recv[2].encode('hex'), 16)
                        high = int(recv[3].encode('hex'), 16)
                        global distance
                        distance = low + high * 256
            res = distance
            #print res
            x =  res * math.cos(math.radians(deze))
            y =  res * math.sin(math.radians(deze))
            xreal = x + 240
            yreal = 320 - y
            pygame.draw.line(screen, (0, 255, 0), (240, 320), (xreal, yreal))

            pygame.time.wait(15)
            pygame.display.flip()
            #print(res)
            pi.set_servo_pulsewidth(GPIO, deze * 11 + 500)
            time.sleep(0.2)
        screen.fill((0,0,0))
        pygame.draw.circle(screen, (0,128,0), (240,320), 100,1)
        pygame.draw.circle(screen, (0,128,0), (240,320), 200,1)
        pygame.draw.circle(screen, (0,128,0), (240,320), 300,1)
        for deze in range(170,10,-5):
            count = ser.in_waiting
            if count > 8:
                    #print count
                    recv = ser.read(9)
                    ser.reset_input_buffer()
                    if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y'
                        low = int(recv[2].encode('hex'), 16)
                        high = int(recv[3].encode('hex'), 16)
                        global distance
                        distance = low + high * 256
            res = distance
            x =  res * math.cos(math.radians(deze))
            y =  res * math.sin(math.radians(deze))
            xreal = x + 240
            yreal = 320 - y
            pygame.draw.line(screen, (0, 255, 0), (240, 320), (xreal, yreal))
            pygame.time.wait(15)
            pygame.display.flip()
            pi.set_servo_pulsewidth(GPIO, deze * 11 + 500)
            time.sleep(0.2)
#pi.set_servo_pulsewidth(GPIO, 0)
Spread the love