WEEK 16-26

from __future__ import division

import smbus

import math

import time

import numpy as np

import matplotlib.pyplot as plt

import pygame

import scipy.fftpack

import scipy

import csv

import xlsxwriter

from threading import Thread

import multiprocessing

from queue import Queue

import sys

import serial

import struct

# Import the PCA9685 module.

import Adafruit_PCA9685

import RPi.GPIO as GPIO

#from Kalman import KalmanAngle

RestrictPitch = True

# Alternatively specify a different address and/or bus:

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

bus = smbus.SMBus(1)   # or bus = smbus.SMBus(0) for older version boards

#DeviceAddress= 0x68   # MPU6050 device address

#DeviceAddress2= 0x69   # MPU6050 device address

GPIO.setmode(GPIO.BOARD) #intrrupt settings for the raspi

GPIO.setup(8,GPIO.OUT,initial=0)

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

# Configure min and max servo pulse lengths

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

servo_max = 450# 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 = 250, 150

angle = 50

radius1 = 2

radius2 =3

X_data = []

Y_data = []

Z_data = []

X_data2= []

Y_data2 = []

Z_data2 = []

sl=[]

sr=[]

n=512 # no of points take

row = 0 #initialize the row and column

col = 0

flag=0

a=0

########+++++++++++++++ configurations to data reading+++++++++++++

dis= [33.0] # the distance from the moter

k= 24# FILE NO 

N= 2 # No of itterations

material = 'Bear can '

#######+++++++++++++++++++++++++++++++++++++++++++

#+++++++ initialize the comport ++++++++++

#arduino = serial.Serial('/dev/ttyUSB0',9600)

#time.sleep (2)

workbook = xlsxwriter.Workbook('ALL'+ str(material)  + str(k)+ 'sensor1.xlsx')

worksheet_x_raw = workbook.add_worksheet()

worksheet_z_raw = workbook.add_worksheet()

worksheet_x_fft = workbook.add_worksheet()

worksheet_z_fft = workbook.add_worksheet()

worksheet_norm = workbook.add_worksheet()

worksheet_fft_add = workbook.add_worksheet()

worksheet_fft_abs_add = workbook.add_worksheet()

worksheet_norm_fft= workbook.add_worksheet()

workbook2 = xlsxwriter.Workbook('ALL'+ str(material)  + str(k)+ 'sensor2.xlsx')

worksheet_x_raw2 = workbook2.add_worksheet()

worksheet_z_raw2 = workbook2.add_worksheet()

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

class mpu6050:

    address = None

    bus = None

    # Scale Modifiers

    ACCEL_SCALE_MODIFIER_16G = 2048.0

    # Pre-defined ranges

    ACCEL_RANGE_16G = 0x18

    # MPU-6050 Registers

    PWR_MGMT_1 = 0x6B

    PWR_MGMT_2 = 0x6C

    ACCEL_XOUT0 = 0x3B

    #ACCEL_YOUT0 = 0x3D

    ACCEL_ZOUT0 = 0x3F

    ACCEL_CONFIG = 0x1C

    FILT_REG = 0X1A

    def __init__(self, address, bus=1):

        self.address = address

        self.bus = smbus.SMBus(bus)

        # Wake up the MPU-6050 since it starts in sleep mode

        self.bus.write_byte_data(self.address, self.PWR_MGMT_1, 0x00)

    # I2C communication methods

    def read_i2c_word(self, register):

        """Read two i2c registers and combine them.

        register -- the first register to read from.

        Returns the combined read results.

        """

        # Read the data from the registers

        high = self.bus.read_byte_data(self.address, register)

        low = self.bus.read_byte_data(self.address, register + 1)

        value = (high << 8) + low

        if (value >= 0x8000):

            return -((65535 - value) + 1)

        else:

            return value

    # MPU-6050 Methods

    def set_accel_range(self, accel_range):

        """Sets the range of the accelerometer to range.

        accel_range -- the range to set the accelerometer to. Using a

        pre-defined range is advised.

        """

        # First change it to 0x00 to make sure we write the correct value later

        #self.bus.write_byte_data(self.address, self.ACCEL_CONFIG,0x00)

        # Write the new range to the ACCEL_CONFIG register

        self.bus.write_byte_data(self.address, self.ACCEL_CONFIG, 0b00011000)

        #self.bus.write_byte_data(self.address, self. FILT_REG, 0b00000000)

    def read_accel_range(self):

        """Reads the range the accelerometer is set to.

        If raw is True, it will return the raw value from the ACCEL_CONFIG

        register

        If raw is False, it will return an integer: -1, 2, 4, 8 or 16. When it

        returns -1 something went wrong.

        """

        raw_data = self.bus.read_byte_data(self.address, self.ACCEL_CONFIG)

        '''

        if raw is True:

            return raw_data

        '''    

        raw_data == self.ACCEL_RANGE_16G

        return 16

    def get_accel_data(self):

        """Gets and returns the X, Y and Z values from the accelerometer.

        If g is True, it will return the data in g

        If g is False, it will return the data in m/s^2

        Returns a dictionary with the measurement results.

        """

        x = self.read_i2c_word(self.ACCEL_XOUT0)

        #y = self.read_i2c_word(self.ACCEL_YOUT0)

        z = self.read_i2c_word(self.ACCEL_ZOUT0)

        accel_scale_modifier = None

        accel_range = self.read_accel_range()

        '''

        if accel_range == self.ACCEL_RANGE_2G:

            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G

        elif accel_range == self.ACCEL_RANGE_4G:

            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_4G

        elif accel_range == self.ACCEL_RANGE_8G:

            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_8G

        el

        '''

        #if accel_range == self.ACCEL_RANGE_16G:

        accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G

        '''    

        else:

            print("Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G")

            #accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G

        '''

        x = x /2048

        #y = y / accel_scale_modifier

        z = z /2048

        #if g is True:

        return {'x': x,  'z': z}

    def read (self):

        self.set_accel_range (mpu1.ACCEL_RANGE_16G )

        self.read_accel_range()

        accel_data_reading = self.get_accel_data()

        return accel_data_reading

    def read2 (self):

        self.set_accel_range (mpu2.ACCEL_RANGE_16G )

        self.read_accel_range()

        accel_data_reading = self.get_accel_data()

        return accel_data_reading

def sensor1_read ():

    mpu1 = mpu6050(0x68)

    sensor1_read  = mpu1.read()

    return sensor1_read

def sensor2_read ():

    mpu2 = mpu6050(0x69)

    sensor2_read  = mpu2.read2()

    return sensor2_read

def norm ():

    accel_xout = sensor1_read ()['x']

    accel_zout = sensor1_read ()['z']

    #sl.append(accel_xout)

    #sr.append(accel_zout)

    #print(accel_zout)

    #print(accel_xout)

    #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,accel_xout)+np.multiply(accel_zout,accel_zout))

    if (norm<6.5 ): #6.5 previous value

        p=0

        #arduino.write(struct.pack('>B',p))

        #print (p)

    else:

        p=1

        GPIO.output(8,GPIO.HIGH)

        #arduino.write(struct.pack('>B',1))

        #print(norm)

        #print('object')

    return p

def increment():

    global flag

    flag = flag+1

    if (flag == N):

        workbook.close()

        workbook2.close()

        sys.exit()

    return flag

def object_detect():

    i=0;

    Fs=1000 # sampling rate

    #cnt=0

    #.................... set the sheets in the exel sheet to save data ..........

    t = time.strftime("%H%M%S")  # get the real time

    for i in range(512):

        accel_xout = sensor1_read ()['x']

        accel_zout = sensor1_read ()['z']

        accel_xout2 = sensor2_read()['x']

        accel_zout2 = sensor2_read()['z']

        #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,accel_xout)+np.multiply(accel_zout,accel_zout))

        X_data.append(accel_xout)

        X_data2.append(accel_xout2)

        # Y_data.append(accel_yout_scaled)

        Z_data.append(accel_zout)

        Z_data2.append(accel_zout2)

        #norm.append(norm_raw)

        # time.sleep(0.0001)

        # print('total time taken ' ,time.time()-start )

# ........................................... FREQUENCY DOMAIN ANALYSIS...........................

        # Ftx = scipy.fft(B) # g+25

        # Fty = scipy.fft(D)#g+25

    Ftx_g = scipy.fft(X_data)  # g

    Ftz_g = scipy.fft(Z_data)  # g

        # Ft_x = Ftx[1:n/2+1]

        # Ft_y = Fty[1:n/2+1]

        # Ft_x = Ftx[1:n//2+1]

        # Ft_y = Fty[1:n//2+1]

    Ft_xg = Ftx_g[1:n // 2 + 1]

    Ft_zg = Ftz_g[1:n // 2 + 1]

        # print ("X axis",abs(Ft_x))

        # print ("Y axis",abs(Ft_y))

    FFT_abs = (abs(Ft_xg) + abs(Ft_zg)) / 2  # add absolute x and z together

        # print(FFT_abs)

    FFT_complex = (Ft_xg + Ft_zg) / 2  # add fft x and y together

        # print (FFT_complex)

   # ..............................................TIME DOMAIN PLOT (g)............................

    # .......................................... SAVE TO THE EXEL ..................................................

    global a

    worksheet_x_raw.write_row(a, col, X_data)  # save x axix raw data to the sheet 1

    worksheet_x_raw.write_row(a, 513, dis)

    worksheet_z_raw.write_row(a, col, Z_data)  # save z axix raw data to the sheet 2

    worksheet_z_raw.write_row(a, 513, dis)  #

    worksheet_x_fft.write_row(a, col, abs(Ft_xg))  # save x axix fft data to the sheet 3

    worksheet_x_fft.write_row(a, 257, dis)

    worksheet_z_fft.write_row(a, col, abs(Ft_zg))  # save z axix fft data to the sheet 4

    worksheet_z_fft.write_row(a, 257, dis)

    # worksheet_norm.write_row(a, col, Norm)  # save norm raw data to the sheet 5

    # worksheet_norm.write_row(a, 513, dis)

    worksheet_fft_add.write_row(a, col, abs(FFT_complex))  # save fft x and fft z add values  data to the sheet 6

    worksheet_fft_add.write_row(a, 513, dis)  #

    worksheet_fft_abs_add.write_row(a, col, FFT_abs)  # save x fft and z axis abs add vales data to the sheet 7

    worksheet_fft_abs_add.write_row(a, 257, dis)

    # worksheet_norm_fft.write_row(a, col, abs(FFT_norm))  # save norm fft data to the sheet 8

    # worksheet_norm_fft.write_row(a, 257, dis)

    worksheet_x_raw2.write_row(a, col, X_data2)  # save x axix raw data to the sheet 1

    worksheet_x_raw2.write_row(a, 513, dis)

    worksheet_z_raw2.write_row(a, col, Z_data2)  # save z axix raw data to the sheet 2

    worksheet_z_raw2.write_row(a, 513, dis)  #

    #plt.show()

    global X_data

    global X_data2

    X_data=[]

    X_data2 = []

    global Z_data

    global Z_data2

    Z_data=[]

    Z_data2 = []

    a= a+1

    print(a)

    return 

#+++++++++++++++++ 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():

    k=[]

    l=[]

    u=0

    coords = 250, 150

    angle = 50

    radius1 = 3

    radius2 =2

    speed = 0.1

    next_tick = 1000

    clock = pygame.time.Clock()

    pwm.set_pwm(0, 0,355) #initilize the two servos

    #pwm.set_pwm(2, 0, 306)

    while True:

        p=norm()

        if (p==0):

            ticks = pygame.time.get_ticks()

            if ticks > next_tick:

                u=u+1

                next_tick += speed

                angle += 0.8

                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)

                #time.sleep(0.001)

                pwm.set_pwm(0, 0, x)

                #pwm.set_pwm(2, 0, y)

                k.append(x)

                l.append(y)

                #print('X',x)

                #print('k',k[u-2])

                #print('Y',y)

                #print('l',l[u-2])

                #time.sleep(0.001)

        else:

            pwm.set_pwm(0, 0, k[u-9])

            #pwm.set_pwm(2, 0, l[u-9])

            object_detect()

            #print('stopped')

            time.sleep(1)

            sensor_read =[]

            X_data=[]

            Z_data=[]

            GPIO.output(8,GPIO.LOW)

            lk= increment()

            #print(lk)

            #sys.exit()

    clock.tick(1000)

    return 

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

#MPU6050 ilk calistiginda uyku modunda oldugundan, calistirmak icin asagidaki komutu veriyoruz:

mpu1 = mpu6050(0x68)

mpu2 = mpu6050(0x69)

pwm.set_pwm(0, 0,355) #initilize the two servos

#pwm.set_pwm(2, 0, 306)

time.sleep(2)

#f=0

searching()