Mori sl-4 turret location decode

component moricchanger                   "This component controls the Mori Auto Tool Changer. M6 calls this";

pin in bit toolchange                   "Receives signal from M6 that tool change required";
pin in s32 toolnumber                   "Receives Tx data from M6 (tool number requested) Only allows 1-10";
pin in s32 currenttoolnumber            "Receives old tool number";
pin out bit toolchanged = false         "Sends signal when tool change finished";

pin out bit delayturretstart = false    "Starts timerdelay component";
pin in bit delayturretdone =false       "Signals timer finished";

pin out bit clampturret = false         " signal to clamp";
pin out bit unclampturret = false         " signal to unclamp";
pin out bit forward = false             "Direction signal";
pin out bit high = false                "Motor command";

pin in signed toolposition = 0          "Connects to mori component for tool position decode";

// Internal and debugging stuff
pin out s32 progress_level = 0;         // tracks the progress of the toolchange, just here so it can be read easily
param rw s32 tnumber = 0;               // Internal toolnumber to allow stop 1 tool prior then slow advance to toolnumber

option singleton yes;                   // makes no sense to have more than one of these components running - only one ATC
function _;
author "ArcEye ";
license "GPL";
;;

FUNCTION(_)
{
    switch (progress_level)
        {
        case 0:  // idle waiting for toolchange request
                if(toolchange && !toolchanged)  // prevent cycling after change done 
                    {
                    if(currenttoolnumber && toolnumber != currenttoolnumber && toolnumber > 0 && toolnumber < 11) // if a valid number
                        {
                        forward = false; // switch off motor if already on
                        tnumber = toolnumber - 1;  // subtract 1 for turrest slow condition 
                        if(!tnumber) tnumber =  10;  

                        high = false;
                        progress_level = 1;
                        break;
                        }
                    else    // if tool requested is out of range set the toolchanged flag and exit
                            // should only get this if tool table has more tools than ATC can have
                            // otherwise emc will error the M6 command
                        {
                        progress_level = 7;
                        forward = false;                // switch off motor if already on
                        high = false;
                        }
                    }
                 if(!toolchange && toolchanged)
                     toolchanged = false;           // reset once toolchange flag reset by system
                     
                 if(toolchange && !currenttoolnumber) // if no tool is set in axis - 
                     {                                // set axis to tool requested so that can work next time
                     forward = false;
                     high = false;
                     progress_level = 7;
                     }
                     
                 break;
                 

        case 1: // programmed delay to allow relays time to change over
                clampturret = false;
                unclampturret = true;
                delayturretstart = true;
                progress_level = 2;
                break;
                
        case 2:  // wait for turret delay
                if(delayturretdone)                
                    {
                    high = true;
                    forward = true;
                    delayturretstart = false;
                    progress_level = 3;
                    }
                break;
                      
        case 3: // Forward move - read the truth table to determine position
                if(toolposition != tnumber)
                    break;
                high = false;
                forward = true;
                progress_level = 4;
                break;
                
        case 4:  // forward move at slow speed
                if(toolposition != toolnumber)  // wait for actual toolnumber
                    break;
                high = false;
                forward = false;
                progress_level = 5;
                break;
                
        case 5: // programmed delay to allow relays time to change over
                unclampturret = false;
                clampturret = true;
                delayturretstart = true;
                progress_level = 6;
                break;
                
        case 6:  // wait for turret delay
                if(delayturretdone)                 
                    {
                    delayturretstart = false;
                    progress_level = 7;
                    }
                break; 
                
        case 7: // clean up ready for next toolchang
                toolchanged = true;   // signal finished
                progress_level = 0;
                break;
                
        case 10:   break;  // should never get here but if we do then loop endlessly doing nothing
                
        default:    
                progress_level = 10;
                rtapi_print_msg(RTAPI_MSG_ERR, "Error state in morichanger - now disabled - unload morichanger");            
                break;
        }
 
}