component servo_turret "servo driven turret comp";


// Inputs
pin	in	bit		enable			"enable/tool change request pin";
pin	in	s32		abs-feedback	"absolute encoder input";
pin	in	float	feedback		"encoder feedback motor";
pin in 	s32		tool-number		"tool request number pin";
pin in	bit		unlocked		"turret unlocked switch";
pin in	bit		clear			"clear error & reset";

// Paramaters
param rw u32		stations = 8		"max station numbers on turret";
param rw u32		timeout = 15		"timeout time, default 15sec";
param rw float 	lift-delay = 0.01 	"delay for settle on lift&drop(sec)";
param rw float	command-inc			"incremental distance for stations";

// Outputs
pin	out	bit		in-position		"tool changed pin";
pin out float	command			"out put position command for PID";
pin out bit		unclamp			"out put bit to unclamp turret";
pin out bit		pid-enable		"enable pin for PID of motor";
pin out bit		motor-direction "Pin for motor direction, 0=fordward, 1=reverse";

// debug
pin out s32  state_  = 0  "state of comp, 0=idle, 1=unclamped, 2=position, 3=clamping, 4=error";
pin out s32 error-code = 0;
// "1=nolift, 2=invlaid tool call, 3=possition call error", 4=Lock Error"
pin in  u32 msg_level = RTAPI_MSG_ERR; // RTAPI_MSG_INFO

// State variables
variable int	last_enable = 0;
variable int	last_clear = 0;

// Delay timers
variable float cycle_timeout;
variable float lift_timeout;

function _  ;
author "Robert Harpham";
// Based on work by Michael Haberler
description """
Servo_turret.comp
This compoent is for interfacing a turret or changer driven by an AC servo motor
with incremental encoder feedback + absolute encoder feedback (BCD or other form)

Incremental distance must be supplied for PID distance calculation.
default timeout = 15seconds
default station numbers = 8
Debug info - 0=idle, 1=unclamped, 2=position, 3=clamping, 4=error

Based on work by Michael Haberler
""";
license "GPLv2 or greater";
;;
//debug defines
#define MS_IDLE	0
#define MS_UNCLAMPED 1
#define MS_POSITION 2
#define MS_CLAMPING 3
#define MS_ERROR 4

//error defines
#define ERR_NO_LIFT	1
#define ERR_INVALID_TOOL_CALL	2
#define ERR_POSITION_ERROR 	3
#define	ERR_LOCK	4

#define MOTOR_FWD 0
#define MOTOR_REV 1

#include <rtapi_math.h>

FUNCTION(_)
{
    int fwd_hops, rev_hops;

    // honor the clear pin even if not enabled.
    if ((clear ^ last_clear) && clear) {
	// positive edge on clear pin.
	// Reset unclamp state, clear error status, set state to idle
	// turn off motor
	error_code = 0;
	unclamp = 0;
	pid_enable = 0;
	state_ = MS_IDLE;
	rtapi_print_msg(msg_level, "cleared");
    }

    if (enable)	{
	if (enable ^ last_enable) {       // positive edge on enable
	    // things to do once on enable
	    error_code = 0;
	    state_ = MS_IDLE;
	    last_clear = clear;
	}

	switch (state_) {

	case MS_IDLE:

	    //check tool call
	    if ((tool_number < 1) || (tool_number > stations)) {
		//send error message
		error_code = ERR_INVALID_TOOL_CALL;
	    } else {
		if (tool_number == abs_feedback) {
		    in_position = 1;
		    error_code = 0;
		} else {
		    in_position = 0;
		    //determin direction
		    if (abs_feedback < tool_number) {
			//go forward
			fwd_hops = tool_number - abs_feedback;
			rev_hops = stations - fwd_hops;
		    } else {
			//go backwards
			rev_hops = abs_feedback - tool_number;
			fwd_hops = stations - rev_hops;
		    }
		    rtapi_print_msg(msg_level, "fwdhops: %d revhops: %d", fwd_hops, rev_hops);

		    if ((fwd_hops > 0) && (rev_hops > 0)) {
			if (fwd_hops < rev_hops) {
			    //set timeout for list
			    lift_timeout = lift_delay;
			    state_ = MS_UNCLAMPED;
			    motor_direction = MOTOR_FWD;
			} else {
			    lift_timeout = lift_delay;
			    state_ = MS_UNCLAMPED;
			    motor_direction = MOTOR_REV;
			}

		    }
		}

	    }
	    break;

	case MS_UNCLAMPED:

	    lift_timeout -= period * 0.000000001;
	    unclamp = 1;

	    //if lifted and seen
	    if (unlocked > 1) {
		// Set Timeout for position
		cycle_timeout = timeout;
		state_ = MS_POSITION;
		rtapi_print_msg(msg_level, "Unlock Seen");
	    }

	    //if timed out something went wrong
	    if (lift_timeout < 0.0) {
		unclamp = 0;
		pid_enable = 0;
		state_ = MS_IDLE;
		error_code = ERR_NO_LIFT;
		rtapi_print_msg(msg_level, "Unlock Timeout");
	    }
	    break;

	case MS_POSITION:

	    cycle_timeout -= period * 0.000000001;

	    if (motor_direction == MOTOR_FWD) {
		command = feedback + command_inc;
		pid_enable = 1;
		rtapi_print_msg(msg_level, "Start forward search");
	    } else {
		command = feedback - command_inc;
		pid_enable = 1;
		rtapi_print_msg(msg_level, "Start backwards search");
	    }

	    if (tool_number == abs_feedback) {
		// target pot reached need to clamp
		//set timeout for drop
		lift_timeout = lift_delay;
		state_ = MS_CLAMPING;
		rtapi_print_msg(msg_level, "poss %d reached, Locking", abs_feedback);
	    }

	    // check timeout
	    if (cycle_timeout < 0.0 ) {
		unclamp = 0;
		pid_enable = 0;
		state_ = MS_IDLE;
		error_code = ERR_POSITION_ERROR;
		rtapi_print_msg(msg_level, "Position Timeout");
	    }

	    break;

	case MS_CLAMPING:
	    lift_timeout -= period * 0.000000001;
	    unclamp = 0;

	    //if locked and seen
	    if (unlocked < 1) {
		pid_enable = 0;
		state_ = MS_IDLE;
		rtapi_print_msg(msg_level, "Lock Seen");
	    }

	    //if timed out something went wrong
	    if (lift_timeout < 0.0) {
		unclamp = 0;
		pid_enable = 0;
		state_ = MS_IDLE;
		error_code = ERR_LOCK;
		rtapi_print_msg(msg_level, "Lock Timeout");
	    }
	    break;

	case MS_ERROR:
	    // a positive edge on clear-index resets state to idle.
	    if ((clear ^ last_clear) && clear) {
		state_ = MS_IDLE;
		error_code = 0;
		pid_enable = 0;
	    }
	    break;
	}
    }	else {
	if (enable ^ last_enable) {       // negative edge on enable
	    // things to do once on disable
	    ;
	}
    }
    last_enable = enable;
    last_clear = clear;

}