WEEK 13/14

from __future__ import division

import smbus

import math

import time

import numpy as np

import matplotlib.pyplot as plt

import pygame

from threading import Thread

import multiprocessing

from queue import Queue

# Import the PCA9685 module.

import Adafruit_PCA9685

# Alternatively specify a different address and/or bus:

pwm = Adafruit_PCA9685.PCA9685(0x40) # pca9685 I2C Address

bus = smbus.SMBus(1)

address = 0x68 #MPU6050 I2C adress

# +++++++++++++++++++++++++++ Configurations ++++++++++++++++++++++++++++

# Configure min and max servo pulse lengths

servo_min = 250  # Min pulse length out of 4096 ---> 150

servo_max = 500# Max pulse length out of 4096 ----> 600

x_min_old =   -88.97162532327404   #18.82269974138087  

x_max_old =   483.97162532327417  #477.17730025861925 

y_min_old =   99.99999999999997 #125.00000000000001

y_max_old =   329.17730025861925 #354.1423954457447 

OldRange_x= (x_max_old-x_min_old)

NewRange_x = (servo_max - servo_min)

OldRange_y = (y_max_old- y_min_old)  

NewRange_y = (servo_max- servo_min)

coords = 150, 150

angle = 70

radius1 = 5

radius2 =2

# ++++++++ mpu6050 configurations +++++++

power_mgmt_1 = 0x6b

power_mgmt_2 = 0x6c

#+++++++++++++++++ functions for mpu6050 +++++++++++++++++++++

def 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 val

def read_word_2c(adr):

 val = read_word(adr)

 if (val >= 0x8000):

    return -((65535 - val) + 1)

 else:

     return val

def norm ():

    accel_xout = read_word_2c(0x3b)

    #accel_yout = read_word_2c(0x3d)

    accel_zout = read_word_2c(0x3f)

    accel_xout_scaled = (accel_xout / 2048.0)

    #accel_yout_scaled = accel_yout / 2048.0

    accel_zout_scaled = (accel_zout / 2048.0)

    norm = np.sqrt(np.multiply(accel_xout_scaled,accel_xout_scaled)+np.multiply(accel_zout_scaled,accel_zout_scaled ))

    if (norm<18):

        p=0

    else:

        p=1

    return p

#+++++++++++++++++ functions for pca9685+++++++++++++++++++++

# Helper function to make setting a servo pulse width simpler.

def set_servo_pulse(channel, pulse):

    pulse_length = 1000000    # 1,000,000 us per second

    pulse_length //= 60       # 60 Hz

    print('{0}us per period'.format(pulse_length))

    pulse_length //= 4096     # 12 bits of resolution

    print('{0}us per bit'.format(pulse_length))

    pulse *= 1000

    pulse //= pulse_length

    pwm.set_pwm(channel, 0, pulse)

# Set frequency to 60hz, good for servos.

pwm.set_pwm_freq(60)

def move_coords(angle, radius1, radius2, coords):

    #def move_coords(angle, radius1, radius2, coords):

    theta = math.radians(angle)

    theta2 = math.radians(angle+20)

    x=coords[0] + radius1 * math.cos(theta)

    y=coords[1] + radius2 * math.sin(theta)

    return x, y

def searching():

    coords = 150, 150

    angle = 70

    radius1 = 5

    radius2 =2

    speed = 0.1

    next_tick = 1000

    clock = pygame.time.Clock()

    while True:

        p=norm()

        if (p==0):

            ticks = pygame.time.get_ticks()

            if ticks > next_tick:

                next_tick += speed

                angle += 1

                coords = move_coords(angle, radius1,radius2, coords)

                x_old =coords[0]

                y_old =coords[1]

                NewValue_x = (((x_old- x_min_old) * NewRange_x) / OldRange_x) +servo_min  

                NewValue_y = (((y_old - y_min_old) * NewRange_y) / OldRange_y) + servo_min 

                x=  int(round(NewValue_x))

                y=  int(round(NewValue_y))

                #print('x=' , x,'y=' , y )

                #print(coords)

                pwm.set_pwm(0, 0, x)

                pwm.set_pwm(2, 0, y)

        else:

            while True:

                print('stopped')

    clock.tick(1000)

    return

#++++++++++++++++++++main loops ++++++++++++++++++++++++++++

while True:

    p=norm()

    print(p)

    searching()