/**************************************************
* Vehicle Move
**************************************************/
public IEnumerator VehicleMove(float x_m, float y_m, float theta_rad)
{
wait_anything = access_db = wait_VehicleMove = true;
time_access = 0.0f;
float[] arg = new float[3] {
x_m, y_m, theta_rad
};
Debug.Log("SendMessage arg: " + x_m.ToString() + ", " + y_m.ToString() + ", " + theta_rad.ToString());
Req_sp5_control srvReq = new Req_sp5_control(1, 16, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_VehicleMove = false;
}
/**************************************************
* Left Gripper Move
**************************************************/
public IEnumerator LeftGripperMove(float theta_rad)
{
wait_anything = access_db = wait_LeftGripperMove = true;
time_access = 0.0f;
float[] arg = new float[3];
arg[0] = theta_rad;
arg[1] = mainSystem.GetConfig().left_gripper_move_speed *Mathf.Deg2Rad;
arg[2] = mainSystem.GetConfig().left_gripper_move_speed *Mathf.Deg2Rad;
Req_sp5_control srvReq = new Req_sp5_control(5, 15, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_LeftGripperMove = false;
}
/**************************************************
* Left Arm Clear Alarm
**************************************************/
public IEnumerator LeftArmClearAlarm()
{
wait_anything = access_db = wait_LeftArmClearAlarm = true;
time_access = 0.0f;
float[] arg = new float[0];
Req_sp5_control srvReq = new Req_sp5_control(3, 0, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_LeftArmClearAlarm = false;
}
/**************************************************
* Right Arm Stop
**************************************************/
public IEnumerator RightArmStop()
{
wait_anything = access_db = wait_RightArmStop = true;
time_access = 0.0f;
float[] arg = new float[0];
Req_sp5_control srvReq = new Req_sp5_control(2, 5, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_RightArmStop = false;
}
/**************************************************
* Read Vehicle Pos
**************************************************/
public IEnumerator ReadVehiclePos()
{
wait_anything = access_db = wait_VehiclePos = true;
time_access = 0.0f;
float[] arg = new float[0];
Req_sp5_control srvReq = new Req_sp5_control(1, 8, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_VehiclePos = false;
}
/**************************************************
* Left Arm Move
**************************************************/
public IEnumerator LeftArmMove(float[] theta_rad)
{
wait_anything = access_db = wait_LeftArmMove = true;
time_access = 0.0f;
float[] arg = new float[8];
arg[0] = theta_rad[0];
arg[1] = theta_rad[1];
arg[2] = theta_rad[2];
arg[3] = theta_rad[3];
arg[4] = theta_rad[4];
arg[5] = theta_rad[5];
arg[6] = theta_rad[6];
arg[7] = mainSystem.GetConfig().left_arm_move_speed *Mathf.Deg2Rad;
string msg = "";
foreach (float n in arg)
{
msg += n.ToString() + ", ";
}
msg = msg.Substring(0, msg.Length - 2);
Debug.Log("SendMessage arg: " + msg);
Req_sp5_control srvReq = new Req_sp5_control(3, 16, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_LeftArmMove = false;
}
/**************************************************
* Right Arm Reset
**************************************************/
public IEnumerator RightArmReset()
{
wait_anything = access_db = wait_RightArmReset = true;
time_access = 0.0f;
float[] arg = new float[8];
arg[0] = mainSystem.GetConfig().right_arm_home_pos[0] * Mathf.Deg2Rad;
arg[1] = mainSystem.GetConfig().right_arm_home_pos[1] * Mathf.Deg2Rad;
arg[2] = mainSystem.GetConfig().right_arm_home_pos[2] * Mathf.Deg2Rad;
arg[3] = mainSystem.GetConfig().right_arm_home_pos[3] * Mathf.Deg2Rad;
arg[4] = mainSystem.GetConfig().right_arm_home_pos[4] * Mathf.Deg2Rad;
arg[5] = mainSystem.GetConfig().right_arm_home_pos[5] * Mathf.Deg2Rad;
arg[6] = mainSystem.GetConfig().right_arm_home_pos[6] * Mathf.Deg2Rad;
arg[7] = mainSystem.GetConfig().right_arm_move_speed *Mathf.Deg2Rad;
string msg = "";
foreach (float n in arg)
{
msg += n.ToString() + ", ";
}
msg = msg.Substring(0, msg.Length - 2);
Debug.Log("SendMessage arg: " + msg);
Req_sp5_control srvReq = new Req_sp5_control(2, 15, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_RightArmReset = false;
}
/**************************************************
* Read Left Gripper State
**************************************************/
public IEnumerator ReadLeftGripperState()
{
wait_anything = access_db = wait_LeftGripperState = true;
time_access = 0.0f;
float[] arg = new float[0] {
};
Req_sp5_control srvReq = new Req_sp5_control(5, 7, arg);
ServiceCaller_sp5_control(srvReq);
while (access_db)
{
yield return(null);
}
while (success_access || abort_access)
{
yield return(null);
}
wait_anything = wait_LeftGripperState = false;
}
public void ServiceCaller_sp5_control(Req_sp5_control srvReq)
{
ServiceCall_sp5_control call = new ServiceCall_sp5_control(srvReq);
wsc.SendOpMsg(call);
}
public ServiceCall_sp5_control(Req_sp5_control args)
{
this.args = args;
}
