Data Sheet

61
ⅱ) SimpleTorqueOnOffandFK Source Description
while(true) {
char temp = _getch();
if(temp == 27)
break
else if(temp == 13) {
if(gbArmTorque) {
ArmComm.Arm_Torque_Off();
std::cout<<"Torque Off"<<std::endl;
gbArmTorque = false
}
else {
ArmComm.Arm_Torque_On();
cout<<"Torque On"<<endl;
if(ArmComm.Arm_Get_JointPosition(&angle_unit) != COMM_RXSUCCESS) {
printf("Communication Error Occurred\n");
}
cout<<"JointAngle is"<<endl;
angle_rad = RobotisArm.Value2Rad(angle_unit);
cout<< angle_rad - gvdAngleGapCalcandDynamixelRad <<endl<<endl;
cout<<"Angle of Dynamixel is"<<endl;
angle_rad = RobotisArm.Value2Rad(angle_unit);
cout<< angle_rad <<endl<<endl;
cout<<"EndEffector's Pose is"<<endl;
Pose3D CurrentPose;
ArmKinematics.Forward(angle_rad
- gvdAngleGapCalcandDynamixelRad, &CurrentPose);
cout<<"x = "<< CurrentPose.x <<endl;
cout<<"y = "<< CurrentPose.y <<endl;
cout<<"z = "<< CurrentPose.z <<endl;
cout<<"roll = "<< CurrentPose.Roll <<endl;
cout<<"pitch = "<< CurrentPose.Pitch <<endl;
cout<<"yaw = "<< CurrentPose.Yaw <<endl<<endl;
gbArmTorque = true
}
}
else
continue
}
The program aborts without starting by pressing the Esc key.
While the program is running press the Enter key to toggle torque between