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
