component beam_pullmax "Two axis beam height controller for the left and right rams on an Ursviken Pullmax press brake";
description """THIS COMPONENT IS UNTESTED, USE AT YOUR OWN RISK\n
Left ram is master, right ram follows\n
TODO: figure out a way to effectively notify the gui user **what** faulted when there is a fault condition preventing things from moving.""";

pin in bit		safety_light_beam_ok			= 0		"true when good";
pin in bit		left_ram_homed					= 0     "true when left ram is homed";
pin in bit		right_ram_homed					= 0     "true when right ram is homed";
pin in bit		backstop_ready					= 0     "true when backgage is in position";
pin in bit		pedal_up						= 0     "signal from foot pedal up position";
pin in bit		pedal_dn						= 0     "signal from foot pedal down position";
pin in bit		auto_up							= 0		"when false, prevents beam from going up automatically when operator releases pedal part way through bend cycle";
pin in float	position_cmd_auto_tdc_ht		= 0		"commanded Top Dead Center in auto mode";
pin in float	position_cmd_auto_slowdown_ht	= 0		"commanded position change over fast to slow speed in auto mode 3-6mm above the workpiece";
pin in float	position_cmd_auto_mute_ht		= 0		"commanded position safety curtain mute point in auto mode 3-6mm above the workpiece";
pin in float	position_cmd_auto_pre_pinch_ht	= 0		"commanded position stop over plate in auto mode";
pin in float	position_cmd_auto_pinch_ht		= 0		"commanded position clamping point in auto mode";
pin in float	position_cmd_auto_post_pinch_ht	= 0		"commanded position clamped point in auto mode about 0.5mm below clamping point";
pin in float	position_cmd_auto_low_speed_ht	= 0		"commanded position change over to low press speed in auto mode";
pin in float	position_cmd_auto_final_speed	= 0		"commanded position change over to final press speed in auto mode about 0.5mm before BDC";
pin in float	position_cmd_auto_bdc_ht		= 0		"commanded Bottom Dead Center in auto mode";
pin in float	position_cmd_auto_lo_hi_speed	= 0		"commanded position change over low to hi return speed in auto mode";
pin in float	position_cmd_homing				= 0		"commanded position in homing mode";
pin in float	backstop_backoff_dist			= 0		"safe backoff distance for backstop after pinch";
pin in float	y0_pos_fb						= 0		"current position of Y0 (left ram)";
pin in float	y1_pos_fb						= 0		"current position of Y1 (right ram)";
pin in float	max_beam_tilt					= 0		"if left to right side position error is greater than this, it is a fault condition";
pin in float	min_y_dist_to_go				= 0		"if y-dist-to-go is less than this value, then motion is considered complete";

pin out float	y_cmd_pos						= 0		"current active position command for beam";
pin out float	y_dist_to_go					= 0		"distance to go for current move";
pin out float	y_tilt_error					= 0		"current following error of Y1 following Y0";
pin out bit 	bend_complete					= 0		"goes true when beam has reached safe height";
pin out bit		post_pinch						= 0		"goes true when plate is pinched; trigger backstop to backoff";
pin out bit		mute							= 0		"true mutes the safety light curtain";
pin out bit		hyd_pump_contactor				= 0		"activates hydraulic pump motor power contactor";
pin out float	coil_4_0						= 0		"output for solenoid coil 4.0";
pin out float	coil_5_0						= 0		"output for solenoid coil 5.0";
pin out bit		coil_6_1						= 0		"output for solenoid coil 6.1";
pin out bit		coil_7_1						= 0		"output for solenoid coil 7.1";
pin out bit		coil_8_1						= 0		"output for solenoid coil 8.1";
pin out float	coil_8_4						= 0		"output for solenoid coil 8.4";
pin out bit		coil_8_5						= 0		"output for solenoid coil 8.5";
pin out bit		coil_9_1						= 0		"output for solenoid coil 9.1";
pin out bit		coil_10_1						= 0		"output for solenoid coil 10.1";
pin out bit		no_fault						= 1		"goes low when fault condition detected";

pin io u32		status							= 0		"""current status of upper beam operation\n
0	= unhomed\n
1	= homed\n
2	= first footpedal activation\n
4	= first footpedal release\n
10	= idling at max height\n
12	= moving down fast from max height to top dead center\n
14	= at top dead center\n
16	= moving down fast from top dead center to slowdown point\n
18	= at slowdown point\n
20	= moving down slow from slowdown point to safety mute point\n
22	= at safety mute point\n
24	= moving down slow from sefety mute point to stop over plate\n
26	= at stop over plate\n
28	= moving down slow from stop over plate to clamping point\n
30	= at clamping point\n
32	= moving down slow from clamping point to clamped point\n
34	= at clamped point\n
36	= moving down slow from clamped point to change over to slow-slow speed\n
38	= moving down slow-slow from slow-slow chang over point to final-slow change over point\n
40	= at slow-slow to final-slow change over point\n
42	= moving down final-slow speed from final-slow change-over point to Bottom Dead Center\n
44	= at Bottom Dead Center\n
46	= decompression
48	= moving up slow from Bottom Dead Center to slow-to-fast-up change-over point\n
50	= at slow-to-fast-up change-over point\n
52	= moving up fast from slow-to-fast-up change-over point to Top Dead Center\n
54	= at Top Dead Center\n
56	= moving up fast from Top Dead Center to max height; next position is status = 10""";

param rw float	bottom_dead_center_dwell		= 0		"dwell time in seconds at bottom dead center";
param rw float	hyd_pump_stop_delay				= 15 "number of seconds to wait before stopping the hyd pump";
param r float	hyd_pump_elapsed_run_time		= 0;
param r bit		hyd_pump_timed_out				= 0;
param r bit		pedal_is_up						= 0;
param r bit		pedal_is_dn						= 0;
param r float	elapsed_dwell_time				= 0;
param r bit		dwell_complete					= 0;
param rw float	footpedal_first_run_debounce	= 0.1;
param r float	footpedal_elapsed_debounce_time	= 0;
param r bit		footpedal_debounce_complete		= 0;
param r bit		hyd_pump_in_use					= 0;

function _;

license "GPL";
author "Titus Newswanger 2025";
;;

//#include "rtapi_math.h"

FUNCTION(_){
	//  ////////////////
	// position calculations
	y_dist_to_go = y0_pos_fb - y_cmd_pos;
	y_tilt_error = y1_pos_fb - y0_pos_fb;
	if(y_tilt_error >= max_beam_tilt){
		no_fault = 0;
	}
	
	//  ////////////////
	// footpedal debounce time delay
	if(footpedal_first_run_debounce < 0) {
		footpedal_first_run_debounce = 0;
			rtapi_print_msg(RTAPI_MSG_WARN,
				"Footpedal debounce time must be positive, resetting to 0");
	}
	if(footpedal_elapsed_debounce_time < 0) {
		footpedal_elapsed_debounce_time = 0;
		rtapi_print_msg(RTAPI_MSG_WARN,
			"Footpedal debounce time rolled over, resetting to 0");
	}
	if((status == 2) && (pedal_is_dn)){
		if(footpedal_elapsed_debounce_time >= footpedal_first_run_debounce){
			footpedal_debounce_complete = 1;
		}
		else{
			footpedal_elapsed_debounce_time += fperiod;
		}
	}
	if((status == 4) && (pedal_is_up)){
		if(footpedal_elapsed_debounce_time >= footpedal_first_run_debounce){
			footpedal_debounce_complete = 1;
		}
		else{
			footpedal_elapsed_debounce_time += fperiod;
		}
	}
	else{
		footpedal_elapsed_debounce_time = 0;
		footpedal_debounce_complete = 0;
	}
	
	//  ////////////////
	// bottom dead center dwell time delay
	if(bottom_dead_center_dwell < 0) {
		bottom_dead_center_dwell = 0;
			rtapi_print_msg(RTAPI_MSG_WARN,
				"Dwell time must be positive, resetting to 0");
	}
	if(elapsed_dwell_time < 0) {
		elapsed_dwell_time = 0;
		rtapi_print_msg(RTAPI_MSG_WARN,
			"Elapsed time rolled over, resetting to 0");
	}
	if(status == 44){
		if(elapsed_dwell_time >= bottom_dead_center_dwell){
			dwell_complete = 1;
		}
		else{
			elapsed_dwell_time += fperiod;
		}
	}
	else{
		elapsed_dwell_time = 0;
		dwell_complete = 0;
	}

	//  ////////////////
	// pedal, works only if pedal is up or down or niether but not both
	if(pedal_up && !pedal_dn){
		pedal_is_up = 1;
	}
	else{
		pedal_is_up = 0;
	}
	if(!pedal_up && pedal_dn){
		pedal_is_dn = 1;
	}
	else{
		pedal_is_dn = 0;
	}
	if(pedal_up && pedal_dn){
		no_fault = 0;
		status = 1;
		pedal_is_up = 0;
		pedal_is_dn = 0;
	}

	//  ////////////////
	// machine status switch
	if(!(left_ram_homed && right_ram_homed)){
		status = 0;
	}
	else{
		switch(status){
			case 0:
				status = 1;
				break;
			case 1:
				hyd_pump_in_use = 0;
				if(pedal_is_dn){
					status = 2;
				}
				break;
			case 2:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && footpedal_debounce_complete){
					status = 4;
				}
				break;
			case 4:
				hyd_pump_in_use = 0;
				if(pedal_is_up && footpedal_debounce_complete){
					status = 10;
				}
				break;
			case 10:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 12;
				}
				break;
			case 12:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 14;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up){
					status = 56;
				}
				else{
					// pause motion
				}
				break;
			case 14:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 16;
				}
				break;
			case 16:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 18;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up){
					status = 52;
				}
				else{
					// pause motion
				}
				break;
			case 18:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 20;
				}
				break;
			case 20:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 22;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 22:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 24;
				}
				break;
			case 24:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 26;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 26:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 28;
				}
				break;
			case 28:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 30;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 30:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 32;
				}
				break;
			case 32:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 34;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 34:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 36;
				}
				break;
			case 36:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 36;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 38:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 40;
				}
				break;
			case 40:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 42;
				}
				if(pedal_is_dn && no_fault){
					hyd_pump_in_use = 1;
				}
				else if(pedal_is_up && auto_up){
					status = 48;
				}
				else{
					// pause motion
				}
				break;
			case 42:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault){
					status = 44;
				}
				break;
			case 44:
				hyd_pump_in_use = 0;
				if(pedal_is_dn && no_fault && dwell_complete){
					status = 46;
				}
				break;
			case 46:		// decompression
				hyd_pump_in_use = 0;
				if(pedal_is_up && no_fault){
					status = 48;
				}
				break;
			case 48:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 50;
				}
				if(pedal_is_up && no_fault){
					hyd_pump_in_use = 1;
				}
				else{
					// pause motion
				}
				break;
			case 50:
				hyd_pump_in_use = 0;
				if(pedal_is_up && no_fault){
					status = 52;
				}
				break;
			case 52:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 54;
				}
				if(pedal_is_up && no_fault){
					hyd_pump_in_use = 1;
				}
				else{
					// pause motion
				}
				break;
			case 54:
				hyd_pump_in_use = 0;
				if(pedal_is_up && no_fault){
					status = 56;
				}
				break;
			case 56:
				if(y_dist_to_go < min_y_dist_to_go){
					status = 10;
				}
				if(pedal_is_up && no_fault){
					hyd_pump_in_use = 1;
				}
				else{
					// pause motion
				}
				break;
		}
	}
}