2dscan.py

#!/usr/bin/python

from Adafruit_PWM_Servo_Driver import PWM
import time
import numpy as np
import cv2
import math
import sys




#create a window
img = np.zeros((480,640,3), np.uint8)
img[:] = (0,0,0)
cv2.circle(img, (320,240), 3, (0,255,0))


class ser():
	def __init__(self):
		# Initialise the PWM device using the default address
		self.pwm = PWM(0x40)
		#self.pwm.setPWMFreq(100) 
		#servo variables
		self.servoMin = 170  # Min pulse length out of 4096
		self.servoMax = 950  # Max pulse length out of 4096
		self.servo_pulse = self.servoMin
		self.direction = 1
		self.step = 10
	def sweep_servo(self):
		 if(self.direction == 1 and self.servo_pulse >= self.servoMax):
		    	self.direction = -1
		 if(self.direction == -1 and self.servo_pulse <= self.servoMin):
		    	self.direction = 1
		
		 self.servo_pulse = self.servo_pulse + (self.direction * self.step)
		 self.servo_pulse = min(self.servoMax, self.servo_pulse)
		 self.servo_pulse = max(self.servoMin, self.servo_pulse)
                 self.pwm.setPWM(0, 0, self.servo_pulse)
		

def plot_point(distance, angle):
    #meters to pixels scalar
    scalar = 50
    pix = distance * scalar
    center = ((int)(pix * math.cos(angle)) + 320, 240 - (int)(pix * math.sin(angle)))
    cv2.circle(img, center, 1, (255,255,255))
    cv2.imshow('map', img)
    cv2.waitKey(1)


 
# Demo callback handler for raw MAVLink messages
def mavrx_debug_handler(message):
    if message.get_type() == 'RANGEFINDER':
	dist = message.distance
	servo_angle = (servo.servo_pulse * math.pi/780) - 17*math.pi/78 #pulse to radians
	print (servo_angle * 180/math.pi)
	plot_point(dist,servo_angle)
	servo.sweep_servo()

    	

# First get an instance of the API endpoint
api = local_connect()
# Get the connected vehicle (currently only one vehicle can be returned).
v = api.get_vehicles()[0]

servo = ser()

# Set MAVLink callback handler (after getting Vehicle instance)
v.set_mavlink_callback(mavrx_debug_handler)

while not api.exit:
        time.sleep(.1)