void runFalconTest()
{
boost::shared_ptr<FalconFirmware> f;
FalconKinematic* k;
double position[3];
int8_t num_falcons = 0;
int status, i;
unsigned int count;
unsigned int error_count = 0;
unsigned int loop_count = 0;
FalconDevice dev;
#if defined(LIBUSB)
std::cout << "Running libusb test" << std::endl;
dev.setFalconComm<FalconCommLibUSB>();
#elif defined(LIBFTDI)
std::cout << "Running libftdi test" << std::endl;
dev.setFalconComm<FalconCommLibFTDI>();
#elif defined(LIBFTD2XX)
std::cout << "Running ftd2xx test" << std::endl;
dev.setFalconComm<FalconCommFTD2XX>();
#else
std::cout << "No communication types built. Please rebuild libnifalcon and make sure comm libs are built." << std::endl;
return 0;
#endif
dev.setFalconFirmware<FalconFirmwareNovintSDK>();
f = dev.getFalconFirmware();
if(!dev.getDeviceCount(num_falcons))
{
std::cout << "Cannot get device count" << std::endl;
return;
}
count = 0;
std::cout << "Falcons found: " << (int)num_falcons << std::endl;
if(num_falcons == 0)
{
std::cout << "No falcons found, exiting..." << std::endl;
return;
}
std::cout << "Opening falcon" << std::endl;
if(!dev.open(0))
{
std::cout << "Cannot open falcon - Error: " << std::endl; // << dev.getErrorCode() << std::endl;
return;
}
std::cout << "Opened falcon" << std::endl;
if(!dev.isFirmwareLoaded())
{
std::cout << "Loading firmware" << std::endl;
for(int i = 0; i < 10; ++i)
{
if(!dev.getFalconFirmware()->loadFirmware(true, NOVINT_FALCON_NVENT_FIRMWARE_SIZE, const_cast<uint8_t*>(NOVINT_FALCON_NVENT_FIRMWARE)))
{
std::cout << "Could not load firmware" << std::endl;
return;
}
else
{
std::cout <<"Firmware loaded" << std::endl;
break;
}
}
if(!dev.isFirmwareLoaded())
{
std::cout << "Firmware didn't load correctly. Try running findfalcons again" << std::endl;
return;
}
}
for(int j = 0; j < 3; ++j)
{
f->setLEDStatus(2 << (j % 3));
for(int i = 0; i < 1000; )
{
if(dev.runIOLoop()) ++i;
else continue;
printf("Loops: %8d | Enc1: %5d | Enc2: %5d | Enc3: %5d \n", (j*1000)+i, f->getEncoderValues()[0], f->getEncoderValues()[1], f->getEncoderValues()[2]);
++count;
}
}
f->setLEDStatus(0);
dev.runIOLoop();
dev.close();
}
int main(int argc, char* argv[])
{
ros::init(argc,argv, "FalconJoystick");
ros::NodeHandle node;
int falcon_int;
bool debug;
//Button 4 is mimic-ing clutch.
bool clutchPressed, coagPressed;
node.param<int>("falcon_number", falcon_int, 0);
node.param<bool>("falcon_debug", debug, false);
node.param<bool>("falcon_clutch", clutchPressed, true);
node.param<bool>("falcon_coag", coagPressed, true);
if(init_falcon(falcon_int))
{
cout << "Falcon Initialised Starting ROS Node" << endl;
m_falconDevice.setFalconKinematic<libnifalcon::FalconKinematicStamper>();
//Start ROS Publisher
ros::Publisher pub = node.advertise<sensor_msgs::Joy>("/falcon/joystick",10);
ros::Rate loop_rate(1000);
while(node.ok())
{
sensor_msgs::Joy Joystick;
std::array<double, 3> prevPos;
Joystick.buttons.resize(1);
Joystick.axes.resize(3);
std::array<double,3> forces;
//Request the current encoder positions:
std::array<double, 3> Pos;
std::array<double, 3> newHome, prevHome;
int buttons;
if(m_falconDevice.runIOLoop())
{
/////////////////////////////////////////////
Pos = m_falconDevice.getPosition(); //Read in cartesian position
buttons = m_falconDevice.getFalconGrip()->getDigitalInputs(); //Read in buttons
//Publish ROS values
Joystick.buttons[0] = buttons;
Joystick.axes[0] = Pos[0];
Joystick.axes[1] = Pos[1];
Joystick.axes[2] = Pos[2];
pub.publish(Joystick);
//TODO if Joystick can subscribe to twist message use those forces instead for haptic feedback
//if
float KpGainX = -200;
float KpGainY = -200;
float KpGainZ = -200;
float KdGainX = -500;
float KdGainY = -500;
float KdGainZ = -500;
// Check if button 4 is pressed, set the forces equal to 0.
if(buttons == 4 || buttons == 2){
if(buttons == 4 && coagPressed == false){
ROS_INFO("Coag Pressed (Button 4)");
coagPressed = true;
}
else if(buttons == 2 && clutchPressed == false){
ROS_INFO("Clutch Pressed (Button 2)");
clutchPressed = true;
}
forces[0] = 0;
forces[1] = 0;
forces[2] = 0;
}
else{
if(coagPressed == true){
ROS_INFO("Coag Released (Button 4)");
coagPressed = false;
newHome = Pos;
}
else if(clutchPressed == true){
ROS_INFO("Clutch Released (Button 2)");
clutchPressed = false;
newHome = Pos;
}
//Simple PD controller
forces[0] = ((Pos[0] - newHome[0]) * KpGainX) + (Pos[0] - prevPos[0])*KdGainX;
forces[1] = ((Pos[1] - newHome[1]) * KpGainY) + (Pos[1] - prevPos[1])*KdGainY;
forces[2] = ((Pos[2] - newHome[2]) * KpGainZ) + (Pos[2] - prevPos[2])*KdGainZ;
}
m_falconDevice.setForce(forces); //Write falcon forces to driver (Forces updated on IO loop) Should run @ ~1kHz
if(debug)
{
cout << "Position= " << Pos[0] <<" " << Pos[1] << " " << Pos[2] << endl;
cout << "newHome = " << newHome[0] <<" " << newHome[1] << " " << newHome[2] << endl;
cout << "Error =" << Pos[0] - newHome[0] <<" " << Pos[1]-newHome[1] << " " << Pos[2] -newHome[2] << endl;
//cout << "Force= " << forces[0] <<" " << forces[1] << " " << forces[2] << endl;
}
prevPos = Pos;
prevHome = newHome;
}
loop_rate.sleep();
}
m_falconDevice.close();
}
return 0;
}
int main(int argc, char* argv[])
{
ros::init(argc,argv, "FalconJSPID");
ros::NodeHandle node;
ros::Rate loop_rate(2000);
for(int i = 0; i < 3; i++)
{
prevError[i] = 0;
integral[i] = 0;
}
//GAIN VALUES
for(int i = 0; i < 3; i++)
{
KpGain[i] = -99; //Proportional Gain. Ku: -300 Tu: 0.167s (Recommended -99)
KiGain[i] = -0.5; //Integral Gain (Recommended -0.5)
KdGain[i] = -3400; //Derivative Gain (Recommended -3400)
}
//Default location. Leg angles in degrees (Falcon will move to this position following homing)
SetPoint[0] = -18.2;
SetPoint[1] = -18.2;
SetPoint[2] = -18.2;
int falcon_int;
int atpos_count;
node.param<int>("falcon_number", falcon_int, 0);
if(init_falcon(falcon_int))
{
ROS_INFO("Falcon Initialised. Starting Falcon Joint Space PID control node");
m_falconDevice.setFalconKinematic<libnifalcon::FalconKinematicStamper>();
ros::Publisher falcon_atpos_pub = node.advertise<std_msgs::Bool>("falcon_atpos", 1);
ros::Subscriber setpoint_sub = node.subscribe("falcon_setpoint", 1, get_setpoint);
while(node.ok())
{
ros::spinOnce(); //Get any new messages
bool atpos = runPID(); //Perform PID control. Returns true when at setpoint, false when in transit
std_msgs::Bool msg;
//If falcon moving, immediately publish not at position. Otherwise, wait 100 loops allowing for oscillation
if (atpos == false)
{
msg.data = atpos;
falcon_atpos_pub.publish(msg);
atpos_count = 0;
}
else
{
atpos_count++;
if(atpos_count >= 100)
msg.data = atpos;
falcon_atpos_pub.publish(msg); //Publish at position after 100 loops
}
while(false == m_falconDevice.getFalconFirmware()->runIOLoop());
loop_rate.sleep();
}
ROS_INFO("Closing connection to Falcon");
m_falconDevice.close();
}
ROS_INFO("Closing Falcon Joint Space PID Control node");
return 0;
}