# compute velocity error in x
error_dy = 0 # actual_values.dy
# Control Design (Simple PI Controller)
#todo resolve get_velocity and get angle error
picar = Picar()
# Control Input Velocity
velocity_output = Picar.get_velocity(
picar, K_p["vel"] * error_distance +
K_d["vel"] * error_velocity) #input meters per seconds output 0-1
# Control Input Steering Angle
steering_angle_output = Picar.get_angle(
picar, K_p["steer"] * error_y +
K_d["steer"] * error_dy) #input degree -output virtual degree
errors = (error_distance, error_velocity, error_y, error_dy)
return steering_angle_output, velocity_output, errors
PD = Controller(3.0, 1.0, 1.0, 0.1)
(steering_angle, velocity, errors) = PD.get_control_output(desired_values,
actual_values,
last_values=None)
print(steering_angle)
print(velocity)
