بازوی رباتیک موازی دلتا توسط سی شارپ | چگونه سینماتیک و دستی انجام دهیم؟

//connect to controller, default IP is 192.168.0.11
ZauxErr = zmcaux.ZAux_OpenEth("192.168.0.11", out g_Handle);
if (0 != ZauxErr)
{
AlmInifFile.Write(DateTime.Now.ToString("F"), "ZAux_OpenEth Execute Error, Error Code:" + ZauxErr.ToString(), "Error Code Info");
}

/************************************************************************************
'Task No.: /
'Function of this function: build robotic arm forward or inverse connection 
'Input: Mode=0—disconnect, 1—inverse connection, 2—forward connection
'Output: /
'Returned Value: 0—success, 1--failure
'Notes: 
‘    *only controller with R is valid    
'    *when in forward, virtual axis MTYPE = 34
‘    *when in inverse, joint axis MTYPE = 34
‘    *when in inverse, keep enable.
**************************************************************************************/
public int ScaraEstab(int Mode)
{
    //update UI interface’s Delta parameters
    ScaraParaWindows.DeltaParaSave();
    //set robotic arm parameters
    SetControPara();
    //save Delta parameters into Ini file
    WriteIniFile();
    //clear controller alarms
    ZauxErr = zmcaux.ZAux_Direct_Single_Datum(g_Handle, 0, 0);
    //update axis list, the axis sequence is J1, J2, Ju, Jz(joint-axis), Vx, Vy, Vr, Vz (virtual-axis)
    int[] mJScaraAxisList = new int[4]; //joint-axis list
    int[] mVScaraAxisList = new int[4]; //virtual-axis list
    for (int i = 0; i < 4; i++)
    {
        mJScaraAxisList[i] = gDeltaAxisList[i];
        mVScaraAxisList[i] = gVAxisList[i];
    }
    //check whether robotic arm parameters are correct
    if ((DeltaR <= 0) || (Deltar <= 0) || (DeltaL1 <= 0) || (DeltaL2 <= 0))
    {
        MessageBox.Show("Delta Robot has Problem, Please Confirm! ");
        return -1;
    }
    //first to save robotic arm structural parameters into controller TABLE register, then it can build robotic arm connection
    //update robotic arm parameters into controller TABLE register
    float[] ScaraParaList = new float[11] { DeltaR, Deltar, DeltaL1, DeltaL2, gMotorPulNum[0] * gReducRatio[0], gMotorPulNum[1] * gReducRatio[1], gMotorPulNum[2] * gReducRatio[2], 0, 0, 0, gMotorPulNum[3] * gReducRatio[3] };
    ZauxErr = zmcaux.ZAux_Direct_SetTable(g_Handle, TableStaraId, 11, ScaraParaList);
    if (0 != ZauxErr)
    {
        return -1;
    }
    if ((1 == Mode) && (0 == ZauxErr)) //build inverse solution for the robot (when the robot parameters are updated successfully)
    {
        //build Scara inverse
        ZauxErr = zmcaux.ZAux_Direct_Connframe(g_Handle, 4, mJScaraAxisList, 14, TableStaraId, 4, mVScaraAxisList);
        if (0 != ZauxErr)
        {
            ProceWindows.WriteLog("Fail to Switch Inverse");
            return -1;
        }
        ProceWindows.WriteLog("Switch to Inverse");
    }
    else if ((2 == Mode) && (0 == ZauxErr)) //build forward solution for the robot (when the robot parameters are updated successfully)
    {
        //build Scara forward
        ZauxErr = zmcaux.ZAux_Direct_Connreframe(g_Handle, 4, mVScaraAxisList, 14, TableStaraId, 4, mJScaraAxisList);
        if (0 != ZauxErr)
        {
            ProceWindows.WriteLog("Fail to Switch Forward");
            return -1;
        }
        ProceWindows.WriteLog("Switch to Forward");
        }
    }
    return 0;
}

/************************************************************************************
'Task No.: /
'Function of this “function”: joint-axis manual inverse motion, press “-“ to call
'Input: /
'Output: / 
'Returned Value: /
'Note: / 
**************************************************************************************/
private void JHandDirRev0_MouseDown(object sender, MouseEventArgs e)
{
    //whether locks it or not, after locked, then it can move
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //if enabled
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Vmtype = 0;//virtual axis MTYPE
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, MainWindows.gVAxisList[0], ref Vmtype); 
        //if it is not forward state
        if (Vmtype != 34)
        {
            //build robotic arm forward connection
            if (0 != MainWindows.ScaraEstab(2))
            {
                //Fail to build the robot connection
                return;
            }
        }
        //which button to be pressed
        int Id = 0;
        for (int i = 0; i < 4; i++)
        {
            if (((Button)sender).Name == ("JHandDirRev" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        TempAxis = MainWindows.gDeltaAxisList[Id];
        //if manual speed is less than 5, it is inching mode
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //set point motion speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
            //send “inverse” motion command
            cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, -1);
        }
        else
        {
            //set inching speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //send inching motion command
            cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, -1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'Task No.: /
'Function of this “function”: joint-axis manual forward motion, press “+“ to call
'Input: /
'Output: / 
'Returned Value: /
'Note: /     
**************************************************************************************/
private void JHandDirFwd0_MouseDown(object sender, MouseEventArgs e)
{
    //whether locks it or not, after locked, then it can move
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //if enabled
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Vmtype = 0;//virtual axis MTYPE
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, MainWindows.gVAxisList[0], ref Vmtype); 
        //if it is not forward state
        if (Vmtype != 34)
        {
            //build robotic arm forward kinematic
            if (0 != MainWindows.ScaraEstab(2))
            {
                //Fail to build the robot connection
                return;
            }
        }
        //which button to be pressed
        int Id = 0;
        for (int i = 0; i < 4; i++)
        {
            if (((Button)sender).Name == ("JHandDirFwd" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        TempAxis = MainWindows.gDeltaAxisList[Id];
        //if manual speed is less than 5, it is inching mode
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //set point motion speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
            //send “forward” motion command
            cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, 1);
        }
        else
        {
            //set inching speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //send inching motion command
            cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, 1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'Task No.: /
'Function of this “function”: joint-axis stop manual motion, call when point button is released
'Input: /
'Output: / 
'Returned Value: /
'Note: /      
**************************************************************************************/
private void JHandDirRev0_MouseUp(object sender, MouseEventArgs e)
{
    //which button to be pressed
    int Id = 0;
    for (int i = 0; i < 4; i++)
    {
        if (((Button)sender).Name == ("JHandDirRev" + i))
        {
            Id = i;
        }
    }
    int TempAxis = 0;
    TempAxis = MainWindows.gDeltaAxisList[Id];
    //if manual speed is less than 5, it is inching mode
    if (5 <= MainWindows.HnadSpeedPerc)
    {
        MainWindows.ZauxErr = cszmcaux.zmcaux.ZAux_Direct_Single_Cancel(MainWindows.g_Handle, TempAxis, 2);
    }
}

/************************************************************************************
'Task No.: /
'Function of this “function”: virtual manual inverse motion, press “-“ to call
'Input: /
'Output: / 
'Returned Value: /
'Note: /     
**************************************************************************************/
private void VHandDirRev0_MouseDown(object sender, MouseEventArgs e)
{
    //whether locks it or not, after locked, then it can move
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //if enabled
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Jmtype = 0;//joint-axis Mtype
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, gDeltaAxisList[0], ref Jmtype);
        //if it is not inverse state
        if (Jmtype != 33)
        {
            //build robotic arm inverse kinematic
            if (0 != MainWindows.ScaraEstab(1))
            {
                //Fail to build the robot connection
                return;
            }
        }
        //which button to be pressed
        int Id = 0;
        for (int i = 0; i < 5; i++)
        {
            if (((Button)sender).Name == ("VHandDirRev" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        //update current robotic arm state
        TempAxis = MainWindows.gVAxisList[Id];
        //if manual speed is less than 5, it is inching mode
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //set point motion speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
            //send “inverse” motion command
           cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, -1); 
        }
        else
        {
            //set inching speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //send inching motion command
           cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, -1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'Task No.: /
'Function of this “function”: joint-axis manual forward motion, press “+“ to call
'Input: /
'Output: / 
'Returned Value: /
'Note: /    
**************************************************************************************/
private void VHandDirFwd0_MouseDown(object sender, MouseEventArgs e)
{
    //whether locks it or not, after locked, then it can move
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //if enabled
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Jmtype = 0;//joint-axis Mtype
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, gDeltaAxisList[0], ref Jmtype);
        //if it is not forward kinematic
        if (Jmtype != 33)
        {
            //build robotic arm forward connection
            if (0 != MainWindows.ScaraEstab(1))
            {
                //Fail to build the robot connection
                return;
            }
        }
        //which button to be pressed
        int Id = 0;
        for (int i = 0; i < 5; i++)
        {
            if (((Button)sender).Name == ("VHandDirFwd" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        //update current robotic arm state
        TempAxis = MainWindows.gVAxisList[Id];
        //if manual speed is less than 5, it is inching mode
        if (5 <= MainWindows.HnadSpeedPerc)
        {
           //set point motion speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
           //send “forward” motion command
           cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, 1); 
        }
        else
        {
            //set inching speed
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //send inching motion command
           cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, 1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'Task No.: /
'Function of this “function”: virtual-axis stop motion, call when the point motion button is released
'Input: /
'Output: / 
'Returned Value: /
'Note: /     
**************************************************************************************/
//stop the motion
private void VHandDirRev0_MouseUp(object sender, MouseEventArgs e)
{
    //which button to be pressed
    int Id = 0;
    for (int i = 0; i < 5; i++)
    {
        if (((Button)sender).Name == ("VHandDirRev" + i))
        {
            Id = i;
        }
    }
    int TempAxis = 0;
    TempAxis = MainWindows.gVAxisList[Id];
    //if manual speed is less than 5, it is inching mode
    if (5 <= MainWindows.HnadSpeedPerc)
    {
        cszmcaux.zmcaux.ZAux_Direct_Single_Cancel(MainWindows.g_Handle, TempAxis, 2);
    }
}