triangulation code.py

# -*- coding: utf-8 -*-
"""Untitled0.ipynb

Automatically generated by Colab.

Original file is located at
    https://colab.research.google.com/drive/16XjkqjVIxo-aFqbwBef9Q2jxFrfh72wf
"""

#outliers filtered out for each separate window of data

import numpy as np
from scipy.optimize import fsolve
import matplotlib.pyplot as plt

def remove_outliers(window):
    Q1 = np.percentile(window, 25)
    Q3 = np.percentile(window, 75)
    IQR = Q3 - Q1
    lower_bound = Q1 - 1.5 * IQR
    upper_bound = Q3 + 1.5 * IQR

    # Return filtered window
    return [x for x in window if lower_bound < x < upper_bound]

def moving_average_with_outlier_removal(data, window_size):
    """Calculate the moving average with outlier removal."""
    ma_values = []

    for i in range(len(data) - window_size + 1):
        window = data[i:i + window_size]
        filtered_window = remove_outliers(window)
        # Calculate the average of the filtered window, handle empty case
        if filtered_window:
            ma_values.append(np.mean(filtered_window))
        else:
            ma_values.append(np.nan)  # Handle case where all values are outliers

    return ma_values

window_size= 10

# Sample data 1
data1 = np.array([7, 8, 7, 8, 6, 7, 7, 5, 8, 6, 7, 8, 4, 5, 6, 7, 1, 2, 5, 7, 8, 100, 12, 6, 2, 5, 6, 7, 3, 4, 7, 8, 6, 9])

# Calculate moving average with outlier removal
ma1 = moving_average_with_outlier_removal(data1, window_size)
print("Moving Average 1 with Outlier Removal:", ma1)

ama1 = np.nanmean(ma1)
print("Average of Moving Averages 1:", ama1)

# Sample data 2
data2 = np.array([2, 3, 4, 3, 2, 3, 4, 1, 2, 6, 3, 9, 1, 2, 4, 3, 2, 5, 3, 6, 3, 4, 2, 3, 4, 5, 15, 2, 3, 5, 4, 3, 2, 3, 6, 2])

# Calculate moving average with outlier removal
ma2 = moving_average_with_outlier_removal(data2, window_size)
print("Moving Average 2 with Outlier Removal:", ma2)

ama2 = np.nanmean(ma2)
print("Average of Moving Averages 2:", ama2)

# Sample data 3
data3 = np.array([13, 14, 12, 11, 9, 8, 10, 12, 13, 14, 15, 12, 1, 23, 12, 13, 14, 15, 10, 9, 8, 9, 10, 11, 12, 12, 13, 14, 15, 16, 19, 12, 13])

# Calculate moving average with outlier removal
ma3 = moving_average_with_outlier_removal(data3, window_size)
print("Moving Average 3 with Outlier Removal:", ma3)

ama3 = np.nanmean(ma3)
print("Average of Moving Averages 3:", ama3)

# Sample data 4
data4 = np.array([13, 14, 12, 11, 9, 8, 10, 12, 13, 14, 15, 12, 1, 23, 12, 13, 14, 15, 10, 9, 8, 9, 10, 11, 12, 12, 13, 14, 15, 16, 19, 12, 13])

# Calculate moving average with outlier removal
ma4 = moving_average_with_outlier_removal(data4, window_size)
print("Moving Average 4 with Outlier Removal:", ma4)

ama4 = np.nanmean(ma4)
print("Average of Moving Averages 4:", ama4)

# Sample data 5
data5 = np.array([13, 14, 12, 11, 9, 8, 10, 12, 13, 14, 15, 12, 1, 23, 12, 13, 14, 15, 10, 9, 8, 9, 10, 11, 12, 12, 13, 14, 15, 16, 19, 12, 13])

# Calculate moving average with outlier removal
ma5 = moving_average_with_outlier_removal(data5, window_size)
print("Moving Average 5 with Outlier Removal:", ma5)

ama5 = np.nanmean(ma5)
print("Average of Moving Averages 3:", ama5)

# stop here

# Define the system of equations
def my_system(vars):
    x = vars[0]
    y = vars[1]
    z = vars[2]

    l = 1  # Length in meter
    w = 1  # Width in meter
    h = 1  # Height in meter (not used in the equations)

    d1 = ama1
    d2 = ama2
    d3 = ama3
    d4 = ama4
    d5 = ama5

    F = np.zeros(5)
    F[0] = np.sqrt(x**2 + y**2 + z**2)-d1
    F[1] = np.sqrt(x**2 + (y - w)**2 + z**2)-d2
    F[2] = np.sqrt((x + l)**2 + y**2 + z**2)-d3
    F[3] = np.sqrt(x**2 + (y + w)**2 + z**2)-d4
    F[4] = np.sqrt((x + l)**2 + (y + w)**2 + z**2)-d5

    return F

# Initializing reference points
l = 1  # length in meter
w = 1  # width in meter
h = 1  # height in meter

ref1 = np.array([l, 0, 0])
ref2 = np.array([0, w, 0])
ref3 = np.array([0, 0, 0])

# Solve for the system of equations and unknown point
initial_guess = [0.5, 0.5, 0.5, 0, 0]  # Modify this based on expected solutions
solution = fsolve(my_system, initial_guess)
unknown_point = np.round(solution)

# 3D plot
vec_x = [ref1[0], ref2[0], ref3[0]]
vec_y = [ref1[1], ref2[1], ref3[1]]
vec_z = [ref1[2], ref2[2], ref3[2]]

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

ax.scatter(vec_x, vec_y, vec_z, marker='o', label='Reference Points')
ax.scatter(unknown_point[0], unknown_point[1], unknown_point[2], marker='o', color='red', label='Unknown Point')

# Labels and legend
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
ax.legend()

# Show plot
plt.show()

# 3D coordinate to polar spherical coordinates
def cartesian_to_spherical(x, y, z):
    r = np.sqrt(x**2 + y**2 + z**2)  # radius
    theta = np.arctan2(y, x)          # azimuthal angle (in radians)
    phi = np.arccos(z / r)            # polar angle (in radians)
    return r, theta, phi

# Convert to spherical coordinates
r, theta, phi = cartesian_to_spherical(unknown_point[0], unknown_point[1], unknown_point[2])

# Convert angles from radians to degrees
theta_deg = np.degrees(theta)
phi_deg = np.degrees(phi)

# Print results
print("Radius (r):", r)
print("Theta (degrees):", theta_deg)
print("Phi (degrees):", phi_deg)



!pip3 install RPi.GPIO
import RPi.GPIO as GPIO
import time
import math

# Setup GPIO
GPIO.setmode(GPIO.BCM)

# Define servo GPIO pins
SERVO_X_PIN = 18  # Horizontal servo
SERVO_Y_PIN = 17  # Vertical servo
LASER_PIN = 21     # Laser control pin

# Setup PWM for servos
GPIO.setup(SERVO_X_PIN, GPIO.OUT)
GPIO.setup(SERVO_Y_PIN, GPIO.OUT)
GPIO.setup(LASER_PIN, GPIO.OUT)

servo_x = GPIO.PWM(SERVO_X_PIN, 50)  # 50 Hz frequency
servo_y = GPIO.PWM(SERVO_Y_PIN, 50)  # 50 Hz frequency

servo_x.start(0)
servo_y.start(0)

# Target point in spherical coordinates (r, θ, φ)
target_point = (r, theta_deg, phi_deg)  # r=1.0, θ=45°, φ=30°

# Current position in degrees
current_horizontal_angle = 0
current_vertical_angle = 90  # Start at the top

# Function to convert spherical to servo angles
def spherical_to_angles(spherical):
    r, theta, phi = spherical
    # Convert angles from degrees to radians
    theta_rad = math.radians(theta)
    phi_rad = math.radians(phi)

    # Calculate angles for servos
    horizontal_angle = phi  # Azimuthal angle
    vertical_angle = 90 - theta  # Convert polar angle to vertical angle (0 at the top)

    return horizontal_angle, vertical_angle

# Function to convert angle to PWM duty cycle
def angle_to_duty_cycle(angle):
    duty_cycle = (angle + 90) / 18 + 2  # Adjust based on your servo's range
    return max(0, min(12, duty_cycle))  # Ensure duty cycle is within range

# Function to turn the laser on
def turn_laser_on():
    GPIO.output(LASER_PIN, GPIO.HIGH)  # Turn on the laser
    print("Laser ON")

# Main loop
try:
    # Turn the laser on
    turn_laser_on()

    while True:
        # Convert spherical coordinates to angles
        target_horizontal_angle, target_vertical_angle = spherical_to_angles(target_point)

        # Smooth movement towards the target angles
        step_size = 1  # Degrees to move per iteration
        if current_horizontal_angle < target_horizontal_angle:
            current_horizontal_angle = min(current_horizontal_angle + step_size, target_horizontal_angle)
        elif current_horizontal_angle > target_horizontal_angle:
            current_horizontal_angle = max(current_horizontal_angle - step_size, target_horizontal_angle)

        if current_vertical_angle < target_vertical_angle:
            current_vertical_angle = min(current_vertical_angle + step_size, target_vertical_angle)
        elif current_vertical_angle > target_vertical_angle:
            current_vertical_angle = max(current_vertical_angle - step_size, target_vertical_angle)

        # Convert current angles to duty cycles
        duty_cycle_x = angle_to_duty_cycle(current_horizontal_angle)
        duty_cycle_y = angle_to_duty_cycle(current_vertical_angle)

        # Write to servos
        servo_x.ChangeDutyCycle(duty_cycle_x)
        servo_y.ChangeDutyCycle(duty_cycle_y)

        # Small delay for stability
        time.sleep(0.1)

except KeyboardInterrupt:
    print("Program terminated.")

finally:
    servo_x.stop()
    servo_y.stop()
    GPIO.cleanup()  # Clean up GPIO settings!