Scribd
Upload
38 views4 pages

Quiz3 2v2

The document describes a simulator for controlling a car model using PID controllers. The simulator models the car dynamics and integrates them over time. PID controllers are used to control the throttle and steering based on setpoints and sensor feedback. The simulator can operate in manual or autonomous mode and allows setting a target position for autonomous control.

Uploaded by

Sebastián Lorca
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
38 views4 pages

Quiz3 2v2

The document describes a simulator for controlling a car model using PID controllers. The simulator models the car dynamics and integrates them over time. PID controllers are used to control the throttle and steering based on setpoints and sensor feedback. The simulator can operate in manual or autonomous mode and allows setting a target position for autonomous control.

Uploaded by

Sebastián Lorca
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

#!

/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()

You might also like