PID Controller Play

Understandably most won't dl a zip, so here is the script attached to the balancing arm:

??? HOW ARE WE SUPPOSED TO USE THESE CODE / QUOTE TAGS ????

extends Spatial

onready var o_ray = $Sensor/RayCast


var n_step = 0
var n_step_max = 15

var distances = []

var ball_r = 1.0


var distance : float = 0.0
#var elapsedTime
var distance_previous_error : float
var distance_error : float
var period : int = 50

var kp : float = 0.5
var ki : float = 0.2			#	PAST
var kd : float = 50	#3100.0			#	FUTURE
var distance_setpoint : float = 10.0 - ball_r		#21.0
var PID_p : float
var PID_i : float
var PID_d : float
var PID_total : float


func _ready():
	pass

func _process(delta):
	
	n_step += 1
	if n_step >= n_step_max:
		n_step = 0
		#	DO CALC
		pid_step()
		
		#	RESET
		distances = []
	else:
		distances.append(get_dist())

func calc_dist():
	var sum: int = 0
	var n_samples = len(distances)
	for i in range(n_samples):
		sum = sum + distances[i]		#o_arm.get_dist()		#	analog_a
	
	var adc: float = float(sum) / float(n_samples)
	
	return adc

func pid_step():
	distance = calc_dist()
	
	distance_error = distance_setpoint - distance
	PID_p = kp * distance_error
	var dist_difference : float = distance_error - distance_previous_error
	PID_d = kd * ((distance_error - distance_previous_error) / period)
	
	if -3 < distance_error && distance_error < 3:
		PID_i = PID_i + (ki * distance_error)
	else:
		PID_i = 0.0
	
	PID_total = PID_p + PID_i + PID_d
	#PID_total = remap(PID_total, -150, 150, 0, 150)
	#PID_total = remap(PID_total, -150, 150, 0, 150)
	
	var pid_min = -11.0		#20.0
	var pid_max = 11.0		#160.0
	
	if PID_total < pid_min:
		PID_total = pid_min
	
	if PID_total > pid_max:
		PID_total = pid_max
	
	#myservo.write(PID_total+30)
	set_ang(deg2rad(PID_total))
	distance_previous_error = distance_error
	

func set_ang(n):
	rotation.z = n

func get_ang():
	return rotation.z

func get_dist():
	
	var pos_collide = o_ray.get_collision_point()
	var pos_sensor = o_ray.global_transform.origin
	
	var vdist = pos_sensor.distance_to(pos_collide)
	return vdist