/** Copyright (c) 2012-2014 ASIMoV Robotics Pvt. Ltd. All rights reserved.
* www.asimovrobotics.com
* Filename: CoolArmControl.java
* Description: The CoolArmControl java file uses the Java API's provided by V-Rep to control the
* Cool Arm in the Dynamotion simulation environment. This is also used to control the hardware through
* the Dynamotion software.
*/
package javaAPI;
import coppelia.IntW;
import coppelia.IntWA;
import coppelia.FloatW;
import coppelia.FloatWA;
import coppelia.remoteApi;
public class CoolArmControl {
// Variables
int joint0_Handle;
int status;
int clientID;
remoteApi dynamotionServer;
int connectStatus;
//Functions
// Constructor
public CoolArmControl() {
// TODO Auto-generated constructor stub
clientID = 0;
status = 0;
dynamotionServer = new remoteApi();
joint0_Handle = 0;
}
// Connect to the Dynamotion Server.
public int connect(){
//dynamotionServer.simxFinish(-1); // just in case, close all opened connections
clientID = dynamotionServer.simxStart("127.0.0.1",19999,true,true,5000,5);
//clientID1= dynamotionServer.simxStart("127.0.0.1",20000,true,true,5000,5);
return clientID;
}
/* Each object seen in the Dynamotion software/User Interface has a handler and this handler
* is returned using the getObjectHandle function.
*/
public IntW getObjectHandle(String ObjectName){
int status;
IntW objectHandle = new IntW(1);
status = dynamotionServer.simxGetObjectHandle(clientID,ObjectName,objectHandle,dynamotionServer.simx_opmode_oneshot_wait);
if(status == 0){
return objectHandle;
}
else
{
//System.out.format("Error occurred in getObjectHandle function with status %d\n",status);
return objectHandle;
}
}
/* Joint position from 0-5 of the cool arm 400 can be controlled using
* the function setJointPosition().
*/
public void setJointPosition(IntW objectHandle, float position){
int status;
status = dynamotionServer.simxSetJointPosition(clientID,objectHandle.getValue(),position,dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("setJointPosition() function status %d\n",status);
}
/* The joint positions of the cool arm 400 can be read using the function
* getJointPosition()
*/
public FloatW getJointPosition(IntW objectHandle){
int status;
FloatW position = new FloatW(1);
status = dynamotionServer.simxGetJointPosition(clientID,objectHandle.getValue(),position, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("getJointPosition() function status %d\n",status);
return position;
}
/*
* Hardware access is enabled using this function.
*/
public void enableHardwareMode(){
int status;
IntW hardwareStatus = new IntW(1);
IntW enableHardwareObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", enableHardwareObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enableHardwareMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, enableHardwareObjectHandle.getValue(), 508,sim_buttonproperty_staydown , dynamotionServer.simx_opmode_streaming);
//System.out.format("Enable Hardware Button on(8536) status %d\n",hardwareStatus.getValue());
//System.out.format("Enable Hardware Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, enableHardwareObjectHandle.getValue(), 508, 8536 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Enable Hardware Button on status %d\n",hardwareStatus.getValue());
///System.out.format("Enable Hardware Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",508,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Hardware access is disabled using this function.
*/
public void disableHardwareMode(){
int status;
IntW hardwareStatus = new IntW(1);
IntW enableHardwareObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", enableHardwareObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disableHardwareMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, enableHardwareObjectHandle.getValue(), 508, hardwareStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Enable Hardware Button off(8280) status %d\n",hardwareStatus.getValue());
//System.out.format("Enable Hardware Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, enableHardwareObjectHandle.getValue(), 508, 8280 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Enable Hardware Button off status %d\n",hardwareStatus.getValue());
//System.out.format("Enable Hardware Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",508,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Assistive mode is enabled using this function
*/
public void enableAssistiveMode(){
int status;
IntW assistiveModeStatus = new IntW(1);
IntW assistiveModeObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", assistiveModeObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enableAssistiveMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, assistiveModeObjectHandle.getValue(), 502, assistiveModeStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Assistive Button off(8536) status %d\n",assistiveModeStatus.getValue());
//System.out.format("Assistive Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, assistiveModeObjectHandle.getValue(), 502, 8536 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Assistive Button on status %d\n",assistiveModeStatus.getValue());
//System.out.format("Assistive Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",502,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Assistive mode is disabled using this function
*/
public void disableAssistiveMode(){
int status;
IntW assistiveModeStatus = new IntW(1);
IntW assistiveModeObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", assistiveModeObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disableAssistiveMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, assistiveModeObjectHandle.getValue(), 502, assistiveModeStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Disable Assistive Button off(8280) status %d\n",assistiveModeStatus.getValue());
//System.out.format("Assistive Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, assistiveModeObjectHandle.getValue(), 502, 8280 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Assistive Button off status %d\n",assistiveModeStatus.getValue());
//System.out.format("Assistive Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",502,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Record mode is enabled using this function.
*/
public void enableRecordMode(){
int status;
IntW recordStatus = new IntW(1);
IntW recordObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", recordObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enableRecordMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, recordObjectHandle.getValue(), 504, recordStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Record Button on(8536) status %d\n",recordStatus.getValue());
//System.out.format("Record Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, recordObjectHandle.getValue(), 504, 8536 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Record Button on status %d\n",recordStatus.getValue());
//System.out.format("Record Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",504,dynamotionServer.simx_opmode_oneshot_wait);
//dynamotionServer.simxSetStringSignal(clientID,"filepathrecord",filePathRecordWithName,dynamotionServer.simx_opmode_oneshot);
}
/*
* Record mode is disabled using this function.
*/
public void disableRecordMode(){
int status;
IntW recordStatus = new IntW(1);
IntW recordObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", recordObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disableRecordMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, recordObjectHandle.getValue(), 504, recordStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Record Button off(8280) status %d\n",recordStatus.getValue());
//System.out.format("Record Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, recordObjectHandle.getValue(), 504, 8280 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Record Button off status %d\n",recordStatus.getValue());
//System.out.format("Record Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",504,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Playback mode is enabled using this function.
*/
public void enablePlaybackMode(String filePathPlaybackWithName){
int status;
IntW playBackStatus = new IntW(1);
IntW playBackObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", playBackObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enablePlaybackMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, playBackObjectHandle.getValue(), 506, playBackStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Playback Button on(8536) status %d\n",playBackStatus.getValue());
//System.out.format("Playback Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, playBackObjectHandle.getValue(), 506, 8536 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Playback Button on status %d\n",playBackStatus.getValue());
//System.out.format("Playback Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",506,dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetStringSignal(clientID,"filepath",filePathPlaybackWithName,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Playback mode is disabled using this function.
*/
public void disablePlaybackMode(){
int status;
IntW playBackStatus = new IntW(1);
IntW playBackObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", playBackObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disablePlaybackMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, playBackObjectHandle.getValue(), 506, playBackStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("Playback Button off(8280) status %d\n",playBackStatus.getValue());
//System.out.format("Playback Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, playBackObjectHandle.getValue(), 506, 8280 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("Playback Button off status %d\n",playBackStatus.getValue());
//System.out.format("Playback Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",506,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Remote API Button mode is enabled using this function.
*/
public void enableRemoteJavaApiMode(){
int status;
IntW remoteJavaApiStatus = new IntW(1);
IntW remoteJavaApiObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", remoteJavaApiObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enableRemoteJavaApiMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, remoteJavaApiObjectHandle.getValue(), 506, playBackStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("enableRemoteJavaApiMode Button on(8536) status %d\n",remoteJavaApiStatus.getValue());
//System.out.format("enableRemoteJavaApiMode Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, remoteJavaApiObjectHandle.getValue(), 600, 8536 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("enableRemoteJavaApiMode Button on status %d\n",remoteJavaApiStatus.getValue());
//System.out.format("enableRemoteJavaApiMode Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",600,dynamotionServer.simx_opmode_oneshot_wait);
}
/*
* Remote API Button mode is disabled using this function.
*/
public void disableRemoteJavaApiMode(){
int status;
IntW remoteJavaApiStatus = new IntW(1);
IntW remoteJavaApiObjectHandle = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", remoteJavaApiObjectHandle, dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disableRemoteJavaApiMode() function status %d\n",status);
//int S = dynamotionServer.simxGetUIButtonProperty(clientID, playBackObjectHandle.getValue(), 600, playBackStatus, dynamotionServer.simx_opmode_streaming);
//System.out.format("disableRemoteJavaApiMode Button off(8280) status %d\n",playBackStatus.getValue());
//System.out.format("disableRemoteJavaApiMode Button %d\n",S);
int J = dynamotionServer.simxSetUIButtonProperty(clientID, remoteJavaApiObjectHandle.getValue(), 600, 8280 , dynamotionServer.simx_opmode_oneshot_wait);
//System.out.format("disableRemoteJavaApiMode Button off status %d\n",playBackStatus.getValue());
//System.out.format("disableRemoteJavaApiMode Button working if 0 not working if 1: %d\n",J);
dynamotionServer.simxSetIntegerSignal(clientID,"RemoteAPI_button",600,dynamotionServer.simx_opmode_oneshot_wait);
}
///Added for Setting COM port on 22/7/2014
//
public void setCOMPort(String portnumber){
int status;
IntW COMPort = new IntW(1);
status = dynamotionServer.simxGetUIHandle(clientID, "UI", COMPort, dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetUIButtonLabel(clientID,COMPort.getValue(),1200,portnumber,portnumber,dynamotionServer.simx_opmode_oneshot);
}
/////////Ik functions
public void setEndEffectorPosition(int endeffectorhandler,float x, float y, float z){
float[] EndEffectorPosition = new float[3];
EndEffectorPosition[0] = x;
EndEffectorPosition[1] = y;
EndEffectorPosition[2] = z;
FloatWA position3d = new FloatWA(3);
position3d.setArray(EndEffectorPosition);
dynamotionServer.simxSetObjectPosition(clientID,endeffectorhandler,dynamotionServer.sim_handle_parent,position3d,dynamotionServer.simx_opmode_oneshot_wait);
}
public void setEndEffectorOrientation(int endeffectorhandler,float r, float p, float y){
float[] EndEffectorOrientation = new float[3];
EndEffectorOrientation[0] = r;
EndEffectorOrientation[1] = p;
EndEffectorOrientation[2] = y;
FloatWA orientation3d = new FloatWA(3);
orientation3d.setArray(EndEffectorOrientation);
dynamotionServer.simxSetObjectOrientation(clientID, endeffectorhandler,dynamotionServer.sim_handle_parent,orientation3d,dynamotionServer.simx_opmode_oneshot_wait);
}
public void moveGripper(IntW objectHandle1,IntW objectHandle2, float position){
int status1,status2;
status1 = dynamotionServer.simxSetJointPosition(clientID,objectHandle1.getValue(),position,dynamotionServer.simx_opmode_oneshot_wait);
status2 = dynamotionServer.simxSetJointPosition(clientID,objectHandle2.getValue(),-position,dynamotionServer.simx_opmode_oneshot_wait);
}
public void setJointPositionAndVelocity(float[] pos,float[] vel )
{
dynamotionServer.simxSetFloatSignal(clientID,"j0pos",pos[0],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j1pos",pos[1],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j2pos",pos[2],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j3pos",pos[3],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j4pos",pos[4],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j5pos",pos[5],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j0vel",vel[0],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j1vel",vel[1],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j2vel",vel[2],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j3vel",vel[3],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j4vel",vel[4],dynamotionServer.simx_opmode_oneshot_wait);
dynamotionServer.simxSetFloatSignal(clientID,"j5vel",vel[5],dynamotionServer.simx_opmode_oneshot_wait);
}
}