int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam){
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
double dW,dL,bF;
if(argc == 3){
dL = atof(argv[2]);
dL = dL*0.0254;
dW = atof(argv[1]);
}
else if(argc == 2){
dW = atof(argv[1]);
dL = 0.762;
}else{
dW = 30.0;
dL = 0.762;
}
bF = (dW / dL)*1.35581795;
// Lets prevent the torque fault by limiting acceleration?
//pm.safetyModule->setVelocityLimit(2.0);
// Lets instantiate the system calls
systems::ExposedOutput<cp_type> homePos;
systems::Summer<cp_type> posErr("+-");
systems::Gain<cp_type, double, cf_type> gain(bF);
systems::ToolForceToJointTorques<DOF> tf2jt;
connect(homePos.output, posErr.getInput(0));
connect(wam.toolPosition.output, posErr.getInput(1));
connect(posErr.output, gain.input);
connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
connect(gain.output, tf2jt.input);
homePos.setValueUndefined();
connect(tf2jt.output, wam.input);
wam.gravityCompensate();
/*
std::string line;
bool active = true,set=false;
while(active){
std::getline(std::cin, line);
switch (line[0]) {
case 'x':
case 'q':
active = false;
break;
default:
if (line.size() != 0) {
if(set){
homePos.setValue(wam.getToolPosition());
}else{
homePos.setValueUndefined();
}
set = !set;
}
break;
}
}*/
while(pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE){
waitForEnter();
homePos.setValue(wam.getToolPosition());
printf(">>> Bow Position Locked.");
waitForEnter();
homePos.setValueUndefined();
printf(">>> Bow Position Unlocked.");
}
wam.idle();
// wam.moveHome();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
wam.gravityCompensate();
printMenu();
std::string line;
bool going = true;
while (going) {
printf(">>> ");
std::getline(std::cin, line);
switch (line[0]) {
case 'j':
printf("Holding joint positions.\n");
wam.moveTo(wam.getJointPositions());
break;
case 'p':
printf("Holding tool position.\n");
wam.moveTo(wam.getToolPosition());
break;
case 'o':
printf("Holding tool orientation.\n");
wam.moveTo(wam.getToolOrientation());
break;
case 'b':
printf("Holding both tool position and orientation.\n");
wam.moveTo(wam.getToolPose());
break;
case 'i':
printf("WAM idled.\n");
// Note that this use of the word "idle" does not mean "Shift-idle".
// Calling Wam::idle() will disable any of the controllers that may
// be connected (joint position, tool position, tool orientation,
// etc.) leaving only gravity compensation. (More specifically,
// Wam::idle() disconnects any inputs that were connected using
// Wam::trackReferenceSignal().)
wam.idle();
break;
case 'q':
case 'x':
printf("Quitting.\n");
wam.moveHome();
going = false;
break;
default:
if (line.size() != 0) {
printf("Unrecognized option.\n");
printMenu();
}
break;
}
}
// Release the WAM if we're holding. This is convenient because it allows
// users to move the WAM back to some collapsed position before exiting, if
// they want.
wam.idle();
// Wait for the user to press Shift-idle
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}