;; Inverse Kinematics


;; set the values of angle array to home position
sub anglesHome(var float[5] theta)
	theta[0] = 0
	theta[1] = - 90
	theta[2] = 90
	theta[3] = -90
	theta[4] = 0 
end sub

;; Use the inverse kinematics of the arm to calculate the joint variable
;; param T is the position matrix of the tool
;; param theta is the joint variable array to be determined
sub computeAngles(float [4,4] T, var float[5] theta)

end sub


;; compute the amount each joint needs to be moved
;; param theta is current joint value
;; param new_theta is new joint value
;; param phi is the difference with joint coupling
sub differenceAngles(float[5] theta, float[5] new_theta, var float[5] phi)
	int i
	for i = 0 to 4
		phi[i] = theta[i] - new_theta[i]
	end for

	;; two joint rotate in reverse direction
	phi[0] = -phi[0]
	phi[4] = -phi[4]

	;; coupling
	phi[2] = phi[2] + phi[1]
	phi[3] = phi[3] + phi[2]
end sub

;; move each of the 5 joints by the appropriate amount to put
;; the tool at location give by the place param
;; param theta is the current value of the joints
sub moveAngles(var float[5] theta, float[4,4] place)

end sub

;;
main

end main