private void btnHome_Click(object sender, EventArgs e)
{
// JointTarget jCurPos = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
JointTarget jCurPos = new JointTarget();
jCurPos.FillFromString2("[[0,0,0,0,90,0],[9E9,9E9,9E9,9E9,9E9,9E9]]");
try
{
using (this.master = Mastership.Request(controller.Rapid))
{
RapidData rd = controller.Rapid.GetRapidData("T_ROB1", "Module1", "jTarget");
controller.Rapid.GetTask("T_ROB1").SetProgramPointer("Module1", "moveJoint");
rd.Value = jCurPos;
controller.Rapid.Start();
}
if (this.controller.Rapid.ExecutionStatus != ExecutionStatus.Running)
{
master.Release();
}
}
catch (System.InvalidOperationException ex)
{
MessageBox.Show("Mastership is held by another client." + ex.Message);
master.Release();
master = Mastership.Request(controller);
}
catch (System.Exception ex)
{
MessageBox.Show("Unexpected error occurred: " + ex.Message);
}
}
private void Jogging(string joint, float OffsJointValue)
{
JointTarget jCurPos = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
switch (joint)
{
case "J1":
jCurPos.RobAx.Rax_1 += OffsJointValue;
break;
case "J2":
jCurPos.RobAx.Rax_2 += OffsJointValue;
break;
case "J3":
jCurPos.RobAx.Rax_3 += OffsJointValue;
break;
case "J4":
jCurPos.RobAx.Rax_4 += OffsJointValue;
break;
case "J5":
jCurPos.RobAx.Rax_5 += OffsJointValue;
break;
case "J6":
jCurPos.RobAx.Rax_6 += OffsJointValue;
break;
}
try
{
using (this.master = Mastership.Request(controller.Rapid))
{
RapidData rd = controller.Rapid.GetRapidData("T_ROB1", "Module1", "jTarget");
controller.Rapid.GetTask("T_ROB1").SetProgramPointer("Module1", "moveJoint");
rd.Value = jCurPos;
controller.Rapid.Start();
}
if (this.controller.Rapid.ExecutionStatus != ExecutionStatus.Running)
{
master.Release();
}
}
catch (System.InvalidOperationException ex)
{
MessageBox.Show("Mastership is held by another client." + ex.Message);
master.Release();
master = Mastership.Request(controller);
}
catch (System.Exception ex)
{
MessageBox.Show("Unexpected error occurred: " + ex.Message);
}
}
/// <summary>
/// //重置Task程序指针
/// </summary>
/// <param name="controller">控制器</param>
/// <param name="taskname">任务名称</param>
/// <returns>返回值</returns>
private int RAPID_ProgramReset(Controller controller, string taskname)
{
controller.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser); //登录
if (controller.OperatingMode != ControllerOperatingMode.Auto) //自动模式
{
return(-1);
}
if (!controller.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))//可执行
{
controller.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
try
{
using (Mastership m = Mastership.Request(controller.Rapid)) //写权限
{
controller.Rapid.GetTask(taskname).ResetProgramPointer(); //复位程序指针
m.Release();
}
}
catch (Exception)
{
}
return(0);
}
/// <summary>
/// Method to run the program and returns the action message.
/// </summary>
/// <returns> The action message / status. </returns>
private string StartCommand()
{
// Check the mode of the controller
if (_controller.OperatingMode != ControllerOperatingMode.Auto)
{
return("Controller not set in automatic.");
}
// Check if the motors are enabled
if (_controller.State != ControllerState.MotorsOn)
{
return("Motors not on.");
}
// Execute the program
using (Mastership master = Mastership.Request(_controller))
{
_controller.Rapid.Start(RegainMode.Continue, ExecutionMode.Continuous, ExecutionCycle.Once, StartCheck.CallChain);
// Give back the mastership
master.Release();
}
// Return status message
return("Program started.");
}
/// <summary>
/// 开始程序
/// </summary>
/// <param name="controller">控制器</param>
/// <returns></returns>
public int RAPID_ProgramStart(Controller controller)
{
controller.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser); //登录
if (controller.OperatingMode != ControllerOperatingMode.Auto) //自动模式
{
return(-1);
}
if (controller.State != ControllerState.MotorsOn)//已经上电
{
return(-1);
}
if (!controller.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))//可执行
{
controller.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
try
{
using (Mastership m = Mastership.Request(controller.Rapid)) //写权限
{
controller.Rapid.Start(RegainMode.Continue, ExecutionMode.Continuous); //开始继续运行
m.Release();
}
}
catch (Exception ex)
{
return(-1);
}
return(0);
}
/// <summary>
/// 单点传输
/// </summary>
/// <param name="pq"></param>
internal void writePosQuat(Pose pq)
{
using (Mastership m = Mastership.Request(AbbCtrl.Rapid))
{
abbController.DstPosQuat.Value = pq;
m.Release();
}
}
private int RAPID_ProgramStart(Controller c, out List <string> result)
{
result = new List <string>();
c.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser);
try
{
if (c.OperatingMode != ControllerOperatingMode.Auto)
{
result.Add("[error] Need Auto Mode");
return(-1);
}
if (c.State != ControllerState.MotorsOn)
{
result.Add("[error] Motor need ON");
return(-1);
}
if (!c.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))
{
c.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
using (Mastership m = Mastership.Request(c.Rapid))
{
try
{
c.Rapid.Start();
m.Release();
result.Add("[msg] Program Start");
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
m.Release();
return(-1);
}
}
return(0);
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
return(-1);
}
}
public int RestoreSys(Controller c, out List <string> result, string RestoreDir)
{
result = new List <string>();
c.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser);
try
{
if (c.OperatingMode != ControllerOperatingMode.Auto)
{
result.Add("[error] Need Auto Mode");
return(-1);
}
if (!c.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))
{
c.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
using (Mastership mc = Mastership.Request(c.Configuration),
mr = Mastership.Request(c.Rapid)
)
{
try
{
c.Restore(RestoreDir, RestoreIncludes.All, RestoreIgnores.All);
mc.Release();
mr.Release();
result.Add("[msg] Restore System");
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
mc.Release();
mr.Release();
return(-1);
}
}
return(0);
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
return(-1);
}
}
private int RAPID_ProgramReset(Controller c, out List <string> result, string taskname)
//private int RAPID_ProgramReset(Controller c, out List<string> result, string taskname = "T_ROB1")
{
result = new List <string>();
c.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser);
try
{
if (c.OperatingMode != ControllerOperatingMode.Auto)
{
result.Add("[error] Need Auto Mode");
return(-1);
}
if (!c.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))
{
c.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
using (Mastership m = Mastership.Request(c.Rapid))
{
try
{
c.Rapid.GetTask(taskname).ResetProgramPointer();
result.Add("[msg] Program Reset");
m.Release();
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
m.Release();
return(-1);
}
}
return(0);
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
return(-1);
}
}
public int BackUpSys(Controller c, out List <string> result, string backupDir)
{
result = new List <string>();
c.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser);
try
{
if (c.OperatingMode != ControllerOperatingMode.Auto)
{
result.Add("[error] Need Auto Mode");
return(-1);
}
if (!c.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))
{
c.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
using (Mastership m = Mastership.Request(c.Rapid))
{
try
{
c.Backup(backupDir);
m.Release();
result.Add("[msg] Back Up System");
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
m.Release();
return(-1);
}
}
return(0);
}
catch (Exception ex)
{
result.Add("[error]" + ex.ToString());
return(-1);
}
}
/// <summary>
/// //重置Task程序指针
/// </summary>
/// <param name="controller">控制器</param>
/// <param name="taskname">任务名称</param>
/// <returns>返回值</returns>
public static void RAPID_ProgramReset(Controller controller, string taskname)
{
try
{
using (Mastership m = Mastership.Request(controller)) //写权限
{
controller.Rapid.GetTask(taskname).ResetProgramPointer(); //复位程序指针
m.Release();
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, "错误");
}
}
/// <summary>
/// 执行例行程序之一： 直接执行Rapid中写好的例行程序，不带参数
/// </summary>
/// <param name="controller"> 控制器名称</param>
/// <param name="moduleName"> Rapid 程序中模块名称 </param>
/// <param name="routineName"> Rapid 程序中例行程序名称 </param>
/// <param name="result"> 日志 </param>
/// <returns>成功状态</returns>
public int StartRoutine(Controller controller, string moduleName, string routineName)
{
ABB.Robotics.Controllers.RapidDomain.Task[] tasks = null;
try
{
tasks = controller.Rapid.GetTasks();
if (controller.OperatingMode == ControllerOperatingMode.Auto)
{
//检查任务执行状态，若任务使用中报错： error "SYS_E_EXEC_LEVEL: Operation is illegal at current execution level"
if (tasks[0].ExecutionStatus == TaskExecutionStatus.Running)
{
return(-1);
}
else
{
if (tasks[0].ExecutionStatus == TaskExecutionStatus.Ready || tasks[0].ExecutionStatus == TaskExecutionStatus.Stopped)
{
using (Mastership master = Mastership.Request(controller.Rapid))
{
controller.Rapid.Stop(StopMode.Immediate); //立即停止
tasks[0].SetProgramPointer(moduleName, routineName); //设置程序指针
controller.Rapid.Start(true); //开始程序
master.Release();
}
return(0);
}
}
}
else
{
return(-1);
}
return(-1);
}
catch (System.InvalidOperationException ex)
{
return(-1);
}
catch (System.Exception ex)
{
return(-1);
}
finally
{
}
}
private void button_saveModule_Click(object sender, EventArgs e)
{
try
{
using (Mastership m = Mastership.Request(controller))
{
Module module = (Module)listBox_moduleList.SelectedItem;
module.SaveToFile(controller.FileSystem.RemoteDirectory);
// tRob1.SaveProgramToFile(controller.FileSystem.LocalDirectory);
// + @"\" + textBox_fileName.Text +".mod"
ShowNewMessage("保存完毕啦···目录：" + controller.FileSystem.RemoteDirectory);
m.Release();
button1_Click(null, null); // refresh
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, "保存发生错误");
}
}
/// <summary>
/// Method to stop the program and returns the action message.
/// </summary>
/// <returns> The action message / status. </returns>
private string StopCommand()
{
// Check the mode of the controller
if (_controller.OperatingMode != ControllerOperatingMode.Auto)
{
return("Controller not set in automatic mode.");
}
// Stop the program
using (Mastership master = Mastership.Request(_controller))
{
_controller.Rapid.Stop(StopMode.Instruction);
// Give back the mastership
master.Release();
}
// Return status message
return("Program stopped.");
}
/// <summary>
/// 停止当前Rapid程序
/// </summary>
/// <param name="controller">控制器</param>
public void RAPID_ProgramStop(Controller controller)
{
controller.Logon(ABB.Robotics.Controllers.UserInfo.DefaultUser);
if (controller.OperatingMode != ControllerOperatingMode.Auto)
{
}
if (!controller.AuthenticationSystem.CheckDemandGrant(Grant.ExecuteRapid))
{
controller.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
}
try
{
using (Mastership m = Mastership.Request(controller.Rapid))
{
controller.Rapid.Stop(StopMode.Immediate);
m.Release();
}
}
catch (Exception ex)
{
}
}
/// <summary>
/// 开启主线程
/// </summary>
private void mainThread()
{
int Match_timers = 0; //匹配计数
int Start_Glue_Wait_time = 0; //开始喷胶等待的回合
int Stop_Glue_Wait_time = 0; //停止喷胶等待的回合
GLB.Camera_index = 0; //先从垛区拍照
bool isFinishPutDown = false; //是否完成覆合
Pose mypq = new Pose();
Point3 cls = new Point3();
double[] myQQ = cls.ABC2Q(0, Math.PI, 0);//欧拉角转四元数
//#######################################home#######################################
abbController.StartRoutine(AbbCtrl, "MainModule", "goHome");
WaitForRoutineFinish();//等待Rapid程序执行结束
while (ProgramStart)
{
//#######################################喷胶#######################################
if (GLB.run_mode == 1 && IsStart == true)
{
if (IO_IN_11 == false && isFinishPutDown == false)//滚筒上有海绵,未完成覆合
{
if (ForwardCylinder == "0")
{
ForwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡
}
if (BackwardCylinder == "0")
{
BackwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡阻挡
}
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
Thread.Sleep(200);
abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO13"); //侧推
Thread.Sleep(1000);
abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO13"); //侧推收回
GLB.run_mode = 2; //进入下一工作模式
GLB.Camera_index = 0;
Thread.Sleep(300);
}
else if (IO_IN_11 == false && isFinishPutDown == true) //滚筒上有海绵,完成覆合
{
ForwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡收回
if (BackwardCylinder == "1")
{
BackwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡收回
}
while (IO_IN_11 != true) //直到海绵离开
{
if (IsStart == true && MotorStatus != "1")
{
MotorStatus = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //正转
}
else if (IsStart == false && MotorStatus == "1")
{
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
}
else
{
Thread.Sleep(30);
}
}
isFinishPutDown = false; //清除标志
Thread.Sleep(50); //直到棉完全通过
}
else
{
if (ForwardCylinder != "1")
{
ForwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO12"); //前挡
}
if (MotorStatus != "1")
{
MotorStatus = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //正转:
}
if (Start_Glue_Wait_time == 0 && BackwardCylinder == "1") //后档首次放行
{
BackwardCylinder = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡放行
}
if (IO_IN_10 == false) //检测到有棉
{
if (Start_Glue_Wait_time < 40) //延时开始喷胶
{
Start_Glue_Wait_time++;
Thread.Sleep(10);
}
else
{
Stop_Glue_Wait_time = 0;//清除停止喷胶延时次数
if (BackwardCylinder == "0")
{
BackwardCylinder = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO14"); //后挡阻挡
}
if (Gun1_6 == "0" && GLB.produceSampleList[GLB.produce_index].axisShort > 900)
{
Gun1_6 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO9");//喷胶1.6
}
if (Gun2_3 == "0")
{
Gun2_3 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO10"); //喷胶2.3
}
if (Gun4_5 == "0")
{
Gun4_5 = abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO11"); //喷胶4.5
}
}
}
else
{
if (Stop_Glue_Wait_time < 40) //延时停止喷胶
{
Stop_Glue_Wait_time++;
Thread.Sleep(10);
}
else
{
Start_Glue_Wait_time = 0;//清除开始喷胶延时次数
if (Gun1_6 == "1")
{
Gun1_6 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO9"); //停止喷胶
}
if (Gun2_3 == "1")
{
Gun2_3 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO10");
}
if (Gun4_5 == "1")
{
Gun4_5 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO11");
}
}
}
}
}
else if (GLB.run_mode == 1 && IsStart == false)//中途暂停
{
Thread.Sleep(5);
if (MotorStatus == "1")
{
MotorStatus = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO1"); //停止正转
}
if (Gun1_6 == "1")
{
Gun1_6 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO9"); //停止喷胶
}
if (Gun2_3 == "1")
{
Gun2_3 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO10");
}
if (Gun4_5 == "1")
{
Gun4_5 = abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO11");
}
}
//#######################################图漾相机抓取乳胶棉#######################################
if (GLB.run_mode == 2 && GLB.Match_success == true && IsStart == true) //图漾匹配成功 并且没有暂停
{
GLB.avgAngle.Clear(); //旋转角清空队列
GLB.avgCameraPoint3.Clear(); //相机坐标队列清空
Match_timers = 0; //计数清零
GLB.Camera_index = -1; //停止拍照
GLB.Match_success = false; //清除标志位
//**************乳胶棉目标点**************
myQQ = cls.ABC2Q(-Math.PI / 2 - GLB.device_angl, Math.PI, 0);
mypq.FillFromString2("[[" + GLB.robot_device_point.X + "," + GLB.robot_device_point.Y + "," + GLB.robot_device_point.Z + "]," +
"[" + myQQ[0] + "," + myQQ[1] + "," + myQQ[2] + "," + myQQ[3] + "]]");
writePosQuat(mypq);//传输坐标
Thread.Sleep(50);
abbController.StartRoutine(AbbCtrl, "MainModule", "TakeUpFromZoneToDesk");
WaitForRoutineFinish(); //等待Rapid程序执行结束
GLB.Match_success = false; //清除标志位
GLB.Camera_index = 1; //切换相机
GLB.run_mode = 3;
Thread.Sleep(200);
}
else if (GLB.run_mode == 2 && GLB.Match_success == false && IsStart == true)
{
Match_timers += 1;
if (Match_timers > 20)//二十次匹配不到，暂停
{
this.BeginInvoke(new Action(() =>
{
StartBtn.PerformClick();//点击暂停
Emergency_Stop_Alarm = true;
Thread.Sleep(60);
MessageBox.Show("垛区无法识别到乳胶棉,或者数量过少");
Match_timers = 0;
}));
}
Thread.Sleep(300);
}
////#######################################映美金二次抓取乳胶棉#######################################
if (GLB.run_mode == 3 && GLB.Match_success == true && IsStart == true) //图漾匹配成功 并且没有暂停
{
Match_timers = 0; //计数清零
GLB.Camera_index = -1; //停止拍照
GLB.Match_success = false; //清除标志位
//**************平台乳胶棉目标点**************
myQQ = cls.ABC2Q(-GLB.device_angl, Math.PI, 0);
mypq.FillFromString2("[[" + GLB.robot_device_point.X + "," + GLB.robot_device_point.Y + "," + GLB.robot_device_point.Z + "]," +
"[" + myQQ[0] + "," + myQQ[1] + "," + myQQ[2] + "," + myQQ[3] + "]]");
writePosQuat(mypq);//传输坐标
//产品类型--放置点
abbController.producetypeLocal.FillFromString2(GLB.produce_index.ToString());
using (Mastership m = Mastership.Request(AbbCtrl.Rapid))
{
abbController.produceTypeRobot.Value = abbController.producetypeLocal;
m.Release();
}
Thread.Sleep(50);
abbController.StartRoutine(AbbCtrl, "MainModule", "TakeUpFromDeskToTansLine");
WaitForRoutineFinish(); //等待Rapid程序执行结束
isFinishPutDown = true; //完成覆合
GLB.Camera_index = 0; //切换相机
GLB.run_mode = 1;
Thread.Sleep(200);
}
else if (GLB.run_mode == 3 && GLB.Match_success == false && IsStart == true)
{
Thread.Sleep(500);
abbController.SetIOStatus(AbbCtrl, "Local_IO_0_DO3");//触发相机
Thread.Sleep(50);
abbController.ResetIOStatus(AbbCtrl, "Local_IO_0_DO3");
Match_timers += 1;
if (Match_timers > 10)//十次匹配不到，暂停
{
this.BeginInvoke(new Action(() =>
{
StartBtn.PerformClick();//点击暂停
Thread.Sleep(60);
Emergency_Stop_Alarm = true;
MessageBox.Show("平台上无法识别到乳胶棉");
Match_timers = 0;
}));
}
Thread.Sleep(500);
}
else
{
Thread.Sleep(20);
}
}
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Input variables
GH_Controller controllerGoo = null;
bool connect = false;
bool upload = false;
bool run = false;
bool stop = false;
List <string> programCode = new List <string>();
List <string> systemCode = new List <string>();
// Catch input data
if (!DA.GetData(0, ref controllerGoo))
{
return;
}
if (!DA.GetData(1, ref connect))
{
return;
}
if (!DA.GetData(2, ref upload))
{
return;
}
if (!DA.GetData(3, ref run))
{
return;
}
if (!DA.GetData(4, ref stop))
{
return;
}
if (!DA.GetDataList(5, programCode))
{
programCode = new List <string>()
{
};
}
if (!DA.GetDataList(6, systemCode))
{
systemCode = new List <string>()
{
};
}
base.DestroyIconCache();
// Get controller value
_controller = controllerGoo.Value;
// Connect
if (connect)
{
// Setup the connection
Connect();
// Run the program when toggled
if (run)
{
Run();
}
// Stop the program when toggled
if (stop)
{
Stop();
}
// Upload the code when toggled
if (upload)
{
// Reset program pointer warning
_programPointerWarning = false;
// First stop the current program
Stop();
// Get path for temporary saving of the module files on the local harddrive of the user
// NOTE: This is not a path on the controller, but on the pc of the user
string localDirectory = Path.Combine(DocumentsFolderPath(), "RobotComponents", "temp");
// Check if the directory already exists
if (Directory.Exists(localDirectory))
{
// Delete if it already exists
Directory.Delete(localDirectory, true);
}
// Create new directory
Directory.CreateDirectory(localDirectory);
// Save the RAPID code to the created directory / local folder
SaveModulesToFile(localDirectory, programCode, systemCode);
// Directory to save the modules on the controller
string controllerDirectory = Path.Combine(_controller.FileSystem.RemoteDirectory, "RAPID");
// Module file paths
string filePathProgram;
string filePathSystem;
string directory;
// Upload to the real physical controller
if (_controller.IsVirtual == false)
{
_controller.AuthenticationSystem.DemandGrant(Grant.WriteFtp);
_controller.FileSystem.PutDirectory(localDirectory, "RAPID", true);
directory = controllerDirectory;
}
// Upload to a virtual controller
else
{
directory = localDirectory;
}
// The real upload
using (Mastership master = Mastership.Request(_controller))
{
// Get task
Task[] tasks = _controller.Rapid.GetTasks();
Task task = tasks[0];
// TODO: Make a pick task form? As for pick controller?
// TODO: This can be a solution for multi move with multiple tasks
// Task task = controller.Rapid.GetTask(tasks[0].Name) // Get task with specified name
// Grant acces
_controller.AuthenticationSystem.DemandGrant(Grant.LoadRapidProgram);
// Load the new program from the created file
for (int i = 0; i < systemCode.Count; i++)
{
filePathSystem = Path.Combine(directory, "SystemModule_" + i.ToString() + ".sys");
task.LoadModuleFromFile(filePathSystem, RapidLoadMode.Replace);
}
for (int i = 0; i < programCode.Count; i++)
{
filePathProgram = Path.Combine(directory, "ProgramModule_" + i.ToString() + ".mod");
task.LoadModuleFromFile(filePathProgram, RapidLoadMode.Replace);
}
// Resets the program pointer of this task to the main entry point.
if (_controller.OperatingMode == ControllerOperatingMode.Auto)
{
_controller.AuthenticationSystem.DemandGrant(Grant.ExecuteRapid);
try
{
task.ResetProgramPointer(); // Requires auto mode and execute rapid
_programPointerWarning = false;
}
catch
{
_programPointerWarning = true;
}
}
// Update action status message
if (programCode != null || systemCode != null)
{
_uStatus = "The RAPID code is succesfully uploaded.";
}
else
{
_uStatus = "The RAPID is not uploaded since there is no code defined.";
}
// Give back the mastership
master.Release();
}
// Delete the temporary files
if (Directory.Exists(localDirectory))
{
Directory.Delete(localDirectory, true);
}
}
}
// Disconnect
else
{
// Disconnect
Disconnect();
// Update the satus message when a command wants to be executed without having a connection.
if (run || stop || upload)
{
_uStatus = "Please connect first.";
}
}
// Output message
_msg = $"The remote connection status:\n\nController: {_cStatus}\nActions: {_uStatus}";
if (_programPointerWarning == true)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "The program pointer could not be reset. Check the program modules that are defined" +
" in your controller. Probably you defined two main functions or there are other errors in your RAPID code.");
}
// Output
DA.SetData(0, _msg);
}
private void LinearJogging(string Direction, float OffsJointValue)
{
RobTarget CurrentCartesianPoint = this.controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
double q0, q1, q2, q3;
//double Rz, Ry, Rx;
q0 = CurrentCartesianPoint.Rot.Q1;
q1 = CurrentCartesianPoint.Rot.Q2;
q2 = CurrentCartesianPoint.Rot.Q3;
q3 = CurrentCartesianPoint.Rot.Q4;
//Rz = Math.Atan(2 * (q1 * q2 - q0 * q3) / (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
//Ry = Math.Asin(-2 * (q0 * q2 + q1 * q3));
//Rx = Math.Atan(2 * (q2 * q3 - q0 * q1) / (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
switch (Direction)
{
case "X":
CurrentCartesianPoint.Trans.X += OffsJointValue;
break;
case "Y":
CurrentCartesianPoint.Trans.Y += OffsJointValue;
break;
case "Z":
CurrentCartesianPoint.Trans.Z += OffsJointValue;
break;
case "Q1":
CurrentCartesianPoint.Rot.Q1 += OffsJointValue;
break;
case "Q2":
CurrentCartesianPoint.Rot.Q2 += OffsJointValue;
break;
case "Q3":
CurrentCartesianPoint.Rot.Q3 += OffsJointValue;
break;
case "Q4":
CurrentCartesianPoint.Rot.Q4 += OffsJointValue;
break;
//case "Rx":
// Rx += OffsJointValue;
// break;
//case "Ry":
// Ry += OffsJointValue;
// break;
//case "Rz":
// Rz += OffsJointValue;
// break;
}
//CurrentCartesianPoint.Rot.Q1 = Math.Cos(Rz / 2) * Math.Cos(Ry / 2) * Math.Cos(Rx / 2) - Math.Sin(Rz / 2) * Math.Sin(Ry / 2) * Math.Sin(Rx / 2);
//CurrentCartesianPoint.Rot.Q2 = Math.Cos(Rz / 2) * Math.Cos(Ry / 2) * Math.Sin(Rx / 2) + Math.Sin(Rz / 2) * Math.Sin(Ry / 2) * Math.Cos(Rx / 2);
//CurrentCartesianPoint.Rot.Q3 = Math.Cos(Rz / 2) * Math.Sin(Ry / 2) * Math.Cos(Rx / 2) - Math.Sin(Rz / 2) * Math.Cos(Ry / 2) * Math.Sin(Rx / 2);
//CurrentCartesianPoint.Rot.Q4 = Math.Sin(Rz / 2) * Math.Cos(Ry / 2) * Math.Cos(Rx / 2) + Math.Cos(Rz / 2) * Math.Sin(Ry / 2) * Math.Sin(Rx / 2);
try
{
using (this.master = Mastership.Request(controller.Rapid))
{
RapidData rd = controller.Rapid.GetRapidData("T_ROB1", "Module1", "pTarget");
controller.Rapid.GetTask("T_ROB1").SetProgramPointer("Module1", "MoveLinear");
rd.Value = CurrentCartesianPoint;
controller.Rapid.Start();
}
if (this.controller.Rapid.ExecutionStatus != ExecutionStatus.Running)
{
master.Release();
}
}
catch (System.InvalidOperationException ex)
{
MessageBox.Show("Mastership is held by another client." + ex.Message);
master.Release();
master = Mastership.Request(controller);
}
catch (System.Exception ex)
{
MessageBox.Show("Unexpected error occurred: " + ex.Message);
}
}
