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)