// Dax Phyz PhyzLizp script file.
// Visit http://phyz.ath.cx for more information.

(seq
	(defglobal (PID Vtarget Vcontrol rockets cP cD cI freq)
		(define int_sum 0)
		(define err nil)
		(define lasterr 0)
		(define der nil)
		(define power nil)
		(define i 0)
		(define dt (/ 1 freq))
		(define nextCtrl (+ (clock) dt))
		(define nextDisplay (+ (clock) 1))
		(define now nil)

		(display "%0d%0a*** Entering PID control loop.%0d%0a")
		(while (iswindow PHYZ)
			(setq err (- (PzGetVertX Vcontrol) (PzGetVertX Vtarget)))
			(setq int_sum (+ int_sum err))
			(setq der (- err lasterr))
			(setq power
				(+
					(* cP err)
					(* cD der)
					(* cI int_sum)
				)
			)
			(if (PzGetPhysics)
				(foreach (lambda (R) (PzSetRocketForce R power)) rockets)
				(setq int_sum 0)
			)
			(setq lasterr err)
			(if (< (setq now (clock)) nextCtrl)
				(sleep (- nextCtrl now))
			)
			(setq nextCtrl (max now (+ nextCtrl dt)))

			(if (>= now nextDisplay)
				(seq
					(display (mkstring "err=" (prec -4 err) ", power=" (prec -4 power) "%0d%0a"))
					(setq nextDisplay (max now (+ nextDisplay 1)))
				)
			)
		)
		(display "%0d%0a*** Exiting PID control loop.%0d%0a")
	)

	(systemtext nil)
	(display "%0d%0aPhyz example PID controller%0d%0a")
	(display "%0d%0aSearching for Phyz window...")
	(while (not (PzFindPhyz)) (sleep 0.5))
	(display "%0d%0aPhyz window found.%0d%0a")
	
	(threadeval '(PID 282 31 '(0 1 2 3) 5.0 40.0 0.05 15))
)