#include <PID_v1.h>
#include <SoftwareSerial.h>

// Pin definitions
#define LEFT_MOTOR_PWM 3
#define LEFT_MOTOR_DIR 2
#define RIGHT_MOTOR_PWM 5
#define RIGHT_MOTOR_DIR 4

// Sensor pins (analog)
#define SENSOR_1 A0
#define SENSOR_2 A1
#define SENSOR_3 A2
#define SENSOR_4 A3
#define SENSOR_5 A4

// PID Controller variables
double setpoint = 0;
double input, output;
double kp = 2.0, ki = 0.1, kd = 0.5;
PID pid(&input, &output, &setpoint, kp, ki, kd, DIRECT);

// Base speed
int baseSpeed = 150;

void setup() {
  Serial.begin(9600);
  
  // Initialize motor pins
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  // Initialize PID
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);
  
  Serial.println("Line Following Robot Initialized");
}

void loop() {
  // Read sensor values
  int sensor1 = analogRead(SENSOR_1);
  int sensor2 = analogRead(SENSOR_2);
  int sensor3 = analogRead(SENSOR_3);
  int sensor4 = analogRead(SENSOR_4);
  int sensor5 = analogRead(SENSOR_5);
  
  // Calculate line position (-2 to +2)
  input = calculateLinePosition(sensor1, sensor2, sensor3, sensor4, sensor5);
  
  // Compute PID
  pid.Compute();
  
  // Calculate motor speeds
  int leftSpeed = baseSpeed + output;
  int rightSpeed = baseSpeed - output;
  
  // Constrain speeds
  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);
  
  // Drive motors
  driveMotors(leftSpeed, rightSpeed);
  
  delay(10);
}

double calculateLinePosition(int s1, int s2, int s3, int s4, int s5) {
  // Convert analog readings to digital (0 or 1)
  int sensors[5];
  sensors[0] = (s1 > 500) ? 1 : 0;
  sensors[1] = (s2 > 500) ? 1 : 0;
  sensors[2] = (s3 > 500) ? 1 : 0;
  sensors[3] = (s4 > 500) ? 1 : 0;
  sensors[4] = (s5 > 500) ? 1 : 0;
  
  // Calculate weighted average
  int sum = 0, weightedSum = 0;
  for(int i = 0; i < 5; i++) {
    weightedSum += sensors[i] * (i - 2);
    sum += sensors[i];
  }
  
  if(sum == 0) return 0; // No line detected
  
  return (double)weightedSum / sum;
}

void driveMotors(int leftSpeed, int rightSpeed) {
  // Left motor
  if(leftSpeed >= 0) {
    digitalWrite(LEFT_MOTOR_DIR, HIGH);
    analogWrite(LEFT_MOTOR_PWM, leftSpeed);
  } else {
    digitalWrite(LEFT_MOTOR_DIR, LOW);
    analogWrite(LEFT_MOTOR_PWM, -leftSpeed);
  }
  
  // Right motor
  if(rightSpeed >= 0) {
    digitalWrite(RIGHT_MOTOR_DIR, HIGH);
    analogWrite(RIGHT_MOTOR_PWM, rightSpeed);
  } else {
    digitalWrite(RIGHT_MOTOR_DIR, LOW);
    analogWrite(RIGHT_MOTOR_PWM, -rightSpeed);
  }
}

import serial
import matplotlib.pyplot as plt
import numpy as np
from tkinter import *
from tkinter import ttk

class PIDTuner:
    def __init__(self):
        self.serial_port = None
        self.data_log = []
        self.setup_gui()
        
    def setup_gui(self):
        self.root = Tk()
        self.root.title("PID Tuner for Line Following Robot")
        self.root.geometry("800x600")
        
        # Connection frame
        conn_frame = ttk.Frame(self.root)
        conn_frame.pack(pady=10)
        
        ttk.Label(conn_frame, text="Serial Port:").pack(side=LEFT)
        self.port_entry = ttk.Entry(conn_frame, width=10)
        self.port_entry.pack(side=LEFT, padx=5)
        self.port_entry.insert(0, "COM3")
        
        self.connect_btn = ttk.Button(conn_frame, text="Connect", 
                                    command=self.connect_serial)
        self.connect_btn.pack(side=LEFT, padx=5)
        
        # PID parameter frame
        pid_frame = ttk.LabelFrame(self.root, text="PID Parameters")
        pid_frame.pack(pady=10, padx=10, fill=X)
        
        # Kp
        ttk.Label(pid_frame, text="Kp:").grid(row=0, column=0, sticky=W)
        self.kp_var = DoubleVar(value=2.0)
        self.kp_scale = ttk.Scale(pid_frame, from_=0, to=10, 
                                 variable=self.kp_var, orient=HORIZONTAL)
        self.kp_scale.grid(row=0, column=1, sticky=EW, padx=5)
        self.kp_label = ttk.Label(pid_frame, text="2.0")
        self.kp_label.grid(row=0, column=2)
        
        # Ki
        ttk.Label(pid_frame, text="Ki:").grid(row=1, column=0, sticky=W)
        self.ki_var = DoubleVar(value=0.1)
        self.ki_scale = ttk.Scale(pid_frame, from_=0, to=2, 
                                 variable=self.ki_var, orient=HORIZONTAL)
        self.ki_scale.grid(row=1, column=1, sticky=EW, padx=5)
        self.ki_label = ttk.Label(pid_frame, text="0.1")
        self.ki_label.grid(row=1, column=2)
        
        # Kd
        ttk.Label(pid_frame, text="Kd:").grid(row=2, column=0, sticky=W)
        self.kd_var = DoubleVar(value=0.5)
        self.kd_scale = ttk.Scale(pid_frame, from_=0, to=5, 
                                 variable=self.kd_var, orient=HORIZONTAL)
        self.kd_scale.grid(row=2, column=1, sticky=EW, padx=5)
        self.kd_label = ttk.Label(pid_frame, text="0.5")
        self.kd_label.grid(row=2, column=2)
        
        pid_frame.columnconfigure(1, weight=1)
        
        # Update button
        ttk.Button(pid_frame, text="Send Parameters", 
                  command=self.send_parameters).grid(row=3, column=1, pady=10)
        
        # Bind scale events
        self.kp_scale.bind("<Motion>", self.update_labels)
        self.ki_scale.bind("<Motion>", self.update_labels)
        self.kd_scale.bind("<Motion>", self.update_labels)
        
    def connect_serial(self):
        try:
            port = self.port_entry.get()
            self.serial_port = serial.Serial(port, 9600, timeout=1)
            self.connect_btn.config(text="Connected", state=DISABLED)
            print(f"Connected to {port}")
        except Exception as e:
            print(f"Connection failed: {e}")
            
    def update_labels(self, event=None):
        self.kp_label.config(text=f"{self.kp_var.get():.2f}")
        self.ki_label.config(text=f"{self.ki_var.get():.2f}")
        self.kd_label.config(text=f"{self.kd_var.get():.2f}")
        
    def send_parameters(self):
        if self.serial_port and self.serial_port.is_open:
            kp = self.kp_var.get()
            ki = self.ki_var.get()
            kd = self.kd_var.get()
            
            command = f"PID:{kp:.2f},{ki:.2f},{kd:.2f}\n"
            self.serial_port.write(command.encode())
            print(f"Sent: {command.strip()}")
        
    def run(self):
        self.root.mainloop()
        
if __name__ == "__main__":
    tuner = PIDTuner()
    tuner.run()