#!

/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 3 15:41:22 2024

@author: sebastian
"""

import numpy as np
import pygame as pg
from scipy.integrate import solve_ivp
import math

x0 = np.array([0,0,0,0,0])

XMAX = 1080
YMAX = 720

auto_mode = False

running = True

xk = x0
acc_des = 0
omega_des = 0

tf = 1000
Ts = .1

t = 0

class PIDController:
def __init__(self, kp, ki, kd):
self.kp = kp
self.ki = ki
self.kd = kd
self.prev_error = 0.0
self.integral = 0.0

def update(self, setpoint, measured_value, dt):

error = setpoint - measured_value
self.integral += error * dt
derivative = (error - self.prev_error) / dt
output = self.kp * error + self.ki * self.integral + self.kd * derivative
self.prev_error = error
return output

class CarSimulator:
def __init__(self):
# Car parameters
self.mass = 30.0 # kg
self.length = 8 # meters
self.width = .5 # meters
self.radius = .2
self.inertia = 1
self.b = .3
self.c = .4
self.target = np.array([0,0])
self.auto_mode = False
 # PID controller gains
self.throttle_pid = PIDController(kp=.00001, ki=0.0001, kd=0.000005)
self.auto_throttle_pid = PIDController(kp=.04, ki=0.000001, kd=0.05)
self.steering_pid = PIDController(kp=.00001, ki=0.000001, kd=0.00005)
self.auto_steering_pid = PIDController(kp=.1, ki=0.000001, kd=0.0005)

# Initial state
self.state = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) # [x, y, theta, v, omega]

def dynamics(self, t, state, throttle, steering):

# Unpack state variables
x, y, theta, v, omega = state

# Compute derivatives of state variables

dxdt = v * np.cos(theta)
dydt = v * np.sin(theta)
dthetadt = omega
dvdt = (throttle + steering) / (self.mass * self.radius) - (self.c * v)
omega = (throttle - steering) * self.width / (2 * self.radius *
self.inertia) - self.b * omega / self.inertia

return np.array([dxdt, dydt, dthetadt, dvdt, omega])

def update(self, throttle, steering, dt):

#self.input_manager.update(i_throttle, i_steering)

if self.auto_mode:
error_x = self.target[0] - self.state[0]
error_y = self.target[1] - self.state[1]

target_angle = math.atan2(error_y, error_x)

target_norm = np.sqrt(self.target[0]**2 + self.target[1]**2)

# Calculate control inputs (throttle and steering) using PID
controllers
throttle_force = self.auto_throttle_pid.update(target_norm,
np.sqrt(self.state[0]**2+self.state[1]**2), dt)
steering_force = self.auto_steering_pid.update(target_angle,
self.state[2], dt)

torque_left, torque_right = self.compute_torque(throttle_force,

steering_force)
sol = solve_ivp(self.dynamics, [0, dt], self.state, args=(torque_right,
torque_left), t_eval=[0, dt])
self.state = sol.y[:, -1]
else:

torque_left, torque_right = self.compute_torque(throttle, steering)

# Integrate dynamics using solve_ivp

sol = solve_ivp(self.dynamics, [0, dt], self.state, args=(torque_right,
torque_left), t_eval=[0, dt])
self.state = sol.y[:, -1]

# Update PID controllers

 throttle_force = self.throttle_pid.update(throttle, self.state[3], dt)
steering_force = self.steering_pid.update(steering, self.state[4], dt)

return throttle_force, steering_force

def compute_torque(self, throttle, steering):

tr = (self.mass*self.radius / 2) * throttle +
(self.inertia*self.radius/self.width)*steering
tl = self.mass*self.radius/2* throttle -
self.inertia*self.radius/self.width*steering
return tl, tr
def toggle_drive(self, mode="auto"):
self.auto_mode = False if mode == "manual" else True
def set_target(self, target):
self.target = target

def draw_robot(x, y, angle):

global screen, car
px = int(XMAX / 2 + x)
py = int(YMAX / 2 + y)

px2 = px + int(car.length * np.cos(angle) * 4)

py2 = py + int(car.length * np.sin(angle) * 4)

pg.draw.line(screen, (0, 255, 0), (px, py), (px2, py2), 3)

pg.draw.circle(screen, (255, 0, 0), (px, py), 10)

def update_display():
global xk, screen
x=xk[0]
y = xk[1]
theta = xk[2]
screen.fill("white")

draw_robot(x, y, theta)
pg.display.flip()

def update_state():
global xk, acc_des, omega_des, ti, Ts, car
xk = car.state
acc_des, omega_des = car.update(acc_des, omega_des, Ts)
def handle_keyboard():
global omega_des, acc_des, running, car
# left
for event in pg.event.get():
if event.type == pg.QUIT:
running = False
if event.type == pg.MOUSEBUTTONDOWN:
target_raw = pg.mouse.get_pos()
target = np.array([int(target_raw[0] - XMAX/2), int(target_raw[1] -
YMAX/2)])
car.set_target(target)

keys = pg.key.get_pressed()
if keys[pg.K_LEFT]:
omega_des -= np.radians(3)
# right
if keys[pg.K_RIGHT]:
 omega_des += np.radians(3)
# up
if keys[pg.K_UP]:
acc_des += 10
# down
if keys[pg.K_DOWN]:
acc_des -= 10
if keys[pg.K_m]:
car.toggle_drive("manual")
if keys[pg.K_a]:
car.toggle_drive()

def init_display():
global screen

pg.init()
screen = pg.display.set_mode((XMAX, YMAX))
pg.display.set_caption("Auto")
def main():
# something
global clock, dt, XMAX, YMAX, screen, t, Ts, tf, running, acc_des, omega_des,
car

clock = pg.time.Clock()
dt = 0

init_display()

car = CarSimulator()

while (t <= tf and running):

t += Ts
handle_keyboard()
update_state()
update_display()

dt = clock.tick(60) / 1000

if __name__ == "__main__":
main()