void control_AllegroDogHome::_initHome()
{
if (_home_state_global)
{
printf(">control_AllegroDogHome: ALREADY DONE !!!\n");
return;
}
for (int i=0; i<NUM_CAN_CH; i++)
{
commandDevice(_can[i], CAN_CMD_DISABLE, 0, 0, -1);
//Sleep(500);
commandDevice(_can[i], CAN_CMD_MOTOR_OFF, 0, 0, -1);
commandDevice(_can[i], CAN_CMD_MOTOR_ON, 0, 0, -1);
commandDevice(_can[i], CAN_CMD_OPMODE_POSITION, 0, 0, -1);
commandDevice(_can[i], CAN_CMD_SET_POS_MAX_VELOCITY, _home_speed, 0, -1);
}
AllegroDog_MotorCommand_t motorCmd;
motorCmd.q_des = 0.0f;
motorCmd.qdot_des = 0.0f;
motorCmd.tau_des = 0.0f;
for (int i=0; i<_jdof; i++)
{
_home_state[i] = HOME_INIT;
// write ZERO to motor command
writeDeviceValue(_motor[i], &motorCmd, sizeof(AllegroDog_MotorCommand_t));
}
_home_state_global = HOME_SEARCH_HOME;
_home_step_global = 1;
Sleep(2);
_commandHome(2);
Sleep(2);
_commandHome(5);
Sleep(2);
_commandHome(8);
Sleep(2);
_commandHome(11);
}
void control_AllegroDogHome::_finalizeHome()
{
//for (int i=0; i<NUM_CAN_CH; i++)
//{
//// Sleep(1000);
//// commandDevice(_can[i], CAN_CMD_RESET_ENC, 0, 0, -1);
// //commandDevice(_can[i], CAN_CMD_MOTOR_OFF, 0, 0, -1);
// //commandDevice(_can[i], CAN_CMD_OPMODE_CURRENT, 0, 0, -1);
// for (int j=0; j<JDOF; j+=2)
// {
// Sleep(2);
// commandDevice(_can[i], CAN_CMD_RESET_ENC, 0, 0, j);
// }
//}
/* int i=0;
{
for (int j=0; j<6; j+=2)
{
Sleep(100);
commandDevice(_can[i], CAN_CMD_RESET_ENC, 0, 0, j);
}
}
i=1;
{
for (int j=6; j<JDOF; j+=2)
{
Sleep(100);
commandDevice(_can[i], CAN_CMD_RESET_ENC, 0, 0, j);
}
} */
for (int i=0; i<NUM_CAN_CH; i++)
{
Sleep(2);
commandDevice(_can[i], CAN_CMD_RESET_ENC, 0, 0, -1);
}
printf("Encoder Reset: ALL DONE !!!\n");
/*for (int i=0; i<_jdof; i++)
{
commandDevice(_motor[i], ALLEGRODOGMOTOR_CMD_OPMODE_CUR);
}*/
}
std::string agilent54621aOscilloscopeDevice::execute(int argc, char **argv)
{
string commandString;
string commandValue;
int query = 0; //true (1) or false (0) if the command is expecting a response
double measuredValue = 0;
//bool commandSuccess;
//double commandValue;
//bool outputSuccess;
string result;
//command comes as "attribute value query?"
if(argc == 5)
{
commandValue = argv[4];
commandString = ":SOUR:VOLT:PIEZ " + commandValue;
result = commandDevice(commandString);
}
if(argc == 4)
{
result = queryDevice(":SOUR:VOLT:PIEZ?");
return result;
}
else
return "0"; //command needs to contain 2 pieces of information
/*
if(commandSuccess)
{
outputSuccess = setAttribute(attribute, commandValue); //will only work with attributes that take doubles
if(outputSuccess)
return "1";
else
return "0";
}
else
return "0";
*/
}
void control_AllegroDogHome::_commandHome(int jid)
{
RASSERT(jid >= 0 && jid < JDOF);
commandDevice(_motor[jid], ALLEGRODOGMOTOR_CMD_OPMODE_POS);
commandDevice(_motor[jid], ALLEGRODOGMOTOR_CMD_HOME, (_home_direction[jid] << 16) | (unsigned short)(_home_offset[jid] + _home_offset_tune[jid]));
}
bool agilent54621aOscilloscopeDevice::updateAttribute(string key, string value)
{
//converts desired command into GPIB command string and executes via gpib controller partner device
double tempDouble;
bool successDouble = stringToValue(value, tempDouble);
bool commandSuccess;
bool success = false;
string result;
if(key.compare("GPIB ID") == 0)
{
gpibID = queryDevice("*idn?");
if(gpibID.compare("") == 0)
success = false;
else
success = true;
std::cerr << "Identification: " << gpibID << std::endl;
}
else if(key.compare("Laser Head Operating Hours") == 0)
{
laserHeadHours = queryDevice(":SYST:INF:DHO?");
if(laserHeadHours.compare("") == 0)
success = false;
else
success = true;
std::cerr << "Laser Head Operating Hours: " << laserHeadHours << std::endl;
}
else if(key.compare("Controller Operating Hours") == 0)
{
controllerHours = queryDevice(":SYST:INF:SHO?");
if(controllerHours.compare("") == 0)
success = false;
else
success = true;
std::cerr << "Controller Operating Hours: " << controllerHours << std::endl;
}
else if(key.compare("Laser Wavelength")==0)
{
laserWavelength = queryDevice(":SYST:INF:HWAV?");
if(laserWavelength.compare("") == 0)
success = false;
else
success = true;
std::cerr << "Laser Wavelength: " << laserWavelength << std::endl;
}
else if(key.compare("Piezo Voltage (V)") == 0)
{
bool successPiezoVoltage = stringToValue(value, newPiezoVoltage);
if(successPiezoVoltage && newPiezoVoltage < 117.5 && newPiezoVoltage > 0)
{
std::string piezoCommand = ":SOUR:VOLT:PIEZ " + value;
std::cerr << "piezo_command_str: " << piezoCommand << std::endl;
commandSuccess = commandDevice(piezoCommand);
std::cerr << "device successfully commanded"<< std::endl;
if(commandSuccess)
{
result = queryDevice(":SOUR:VOLT:PIEZ?");
if(result.compare("") == 0)
success = false;
else
{
successPiezoVoltage = stringToValue(result, piezoVoltage);
success = true;
}
}
else
success = false;
}
else
{
std::cerr << "The desired voltage is outside of the allowed range." << std::endl;
success = false;
}
}
else if(key.compare("Power") == 0)
{
if(value.compare("On") == 0)
{
commandSuccess = commandDevice(":OUTP 1");
powerOn = true;
}
else
{
commandSuccess = commandDevice(":OUTP 0");
powerOn = false;
}
if(commandSuccess)
success = true;
/*
if(commandSuccess)
{
result = queryDevice(":OUTP?");
int powerStatus;
bool successPowerStatus = stringToValue(result, powerStatus);
if(result.compare("") == 0)
success = false;
else
{
std::cerr << "Power Status is: " << result << std::endl;
if(powerStatus == 1)
{
success = true;
powerOn = true;
std::cerr << "Laser Turned On" << std::endl;
}
if(powerStatus == 0)
{
success = true;
powerOn = false;
std::cerr << "Laser Turned Off" << std::endl;
}
else
{
success = false;
}
}
}
else
success = false;
*/
}
else if(key.compare("Piezo Gain") == 0)
{
if(value.compare("High") == 0)
{
//set gain to high (25x)
commandSuccess = commandDevice(":CONF:GAIN:HIGH");
std::cerr << "Gain commanded High (25x)." << std::endl;
}
else
{
//set gain to low (1x)
commandSuccess = commandDevice(":CONF:GAIN:LOW");
std::cerr << "Gain commanded Low (1x)." << std::endl;
}
if(commandSuccess)
{
std::string testResult;
result = queryDevice(":CONF:GAIN?");
if(result.compare("") == 0)
success = false;
else
{
testResult.assign(result, 0, 3);
std::cerr << "Piezo Gain is: " << "***" << testResult << "***" << std::endl;
if(testResult.compare("HIG") == 0)
{
success = true;
piezoGainHigh = true;
std::cerr << "set success to true" << std::endl;
}
else if(testResult.compare("LOW") == 0)
{
success = true;
piezoGainHigh = false;
std::cerr << "set success to true" << std::endl;
}
else
{
success = false;
std::cerr << "set success to false" << std::endl;
}
}
}
else
success = false;
}
else if(key.compare("Laser Current (mA)") == 0)
{
bool successLaserCurrent = stringToValue(value, newLaserCurrent);
if(successLaserCurrent && newLaserCurrent < 50.0 && newLaserCurrent > 0)
{
std::string currentCommand = ":SOUR:CURR " + value;
std::cerr << "current_command_str: " << currentCommand << std::endl;
commandSuccess = commandDevice(currentCommand);
std::cerr << "device successfully commanded"<< std::endl;
if(commandSuccess)
{
result = queryDevice(":SOUR:CURR?");
if(result.compare("") == 0)
success = false;
else
{
commandSuccess = stringToValue(result, laserCurrent);
success = true;
}
}
else
success = false;
}
else
{
std::cerr << "The desired current is outside of the allowed range." << std::endl;
success = false;
}
}
return success;
}
