def _setup_():
global rServo
global lServo
GPIO.setmode(GPIO.BCM)
GPIO.setup(RIGHT_SERVO_PIN, GPIO.OUT)
rServo.start(5)
lServo = GPIO.PMW(LEFT_SERVO_PIN, 100)
lServo = GPIO.start(5)
GPIO.setwarnings(False)
GPIO.setup(notReadyLedPin, GPIO.OUT)
GPIO.setup(readyLedPin, GPIO.OUT)
GPIO.setup(triggerPin, GPIO.OUT)
SERVO_CAM0.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, Frame_W)
SERVO_CAM0.set(cv2.cv.CV_CAP - PROP_FRAME_HEIGHT, Frame_H)
SERVO_CAM1.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, Frame_W)
SERVO_CAM1.set(cv2.cv.CV_CAP - PROP_FRAME_HEIGHT, Frame_H)
time.sleep(2)
GPIO.setup(CON_PIN, GPIO.OUT)
GPIO.output(CON_PIN, GPIO.HIGH)
time.sleep(.001)
GPIO.setup(CON_PIN, GPIO.IN)
_offset_calibrate_()
return True
#importing the libraries #from picamera import PiCamera import RPi.GPIO as GPIO import numpy as np import time,os import scanner #setup the GPIO pin for the servo GPIO.setmode(GPIO.BOARD) GPIO.setup(33,GPIO.OUT) # for platform m1=GPIO.PMW(33,50) #50 Hz (20ms PMW period) m1.start(0) # Pins for Motor Driver Inputs Motor1A = 24 Motor1B = 16 Motor1E = 25 def setup(): GPIO.setmode(GPIO.BCM) # GPIO Numbering GPIO.setup(Motor1A,GPIO.OUT) # All pins as Outputs GPIO.setup(Motor1B,GPIO.OUT) GPIO.setup(Motor1E,GPIO.OUT)
time.sleep(.1)
GPIO.setup(pin, GPIO.IN)
while GPIO.input(pin) == GPIO.LOW:
count += 1
count = 106 / count
GPIO.output(CON_PIN, GPIO.LOW)
GPIO.setup(CON_PIN, GPIO.IN)
if pin == YAW:
return (count - min_value) * (180 - (-180) /
((max_value - min_value) + -180))
else:
return (count - min_value) * (90 - 0) / (
(max_value - min_value) + 0)
# startup
_setup_()
lServo = GPIO.PMW(LEFT_SERVO_PIN, 100)
rServo = GPIO.PMW(RIGHT_SERVO_PIN, 100)
# main loop
while True:
leftServo, rightServo = _update_cameras_()
getPitch = _read_arduino_(PITCH)
getYaw = _read_arduino_(YAW)
_set_servos_(rightServo, leftServo)
distance, angle = _distance_and_yaw_cal_(rightServo, leftServo)
targetPitch = _calc_angle_(distance)
canFire = _motor_control_(getYaw, angle, getPitch, targetPitch)
_user_inter_face_(canFire)
