bool init_falcon(int NoFalcon)
{
cout << "Setting up LibUSB" << endl;
m_falconDevice.setFalconFirmware<FalconFirmwareNovintSDK>(); //Set Firmware
m_falconDevice.setFalconGrip<FalconGripFourButton>(); //Set Grip
if(!m_falconDevice.open(NoFalcon)) //Open falcon @ index
{
cout << "Failed to find falcon" << endl;
return false;
}
else
{
cout << "Falcon Found" << endl;
}
//There's only one kind of firmware right now, so automatically set that.
m_falconDevice.setFalconFirmware<FalconFirmwareNovintSDK>();
//Next load the firmware to the device
bool skip_checksum = false;
//See if we have firmware
bool firmware_loaded = false;
firmware_loaded = m_falconDevice.isFirmwareLoaded();
if(!firmware_loaded)
{
cout << "Loading firmware" << endl;
uint8_t* firmware_block;
long firmware_size;
{
firmware_block = const_cast<uint8_t*>(NOVINT_FALCON_NVENT_FIRMWARE);
firmware_size = NOVINT_FALCON_NVENT_FIRMWARE_SIZE;
for(int i = 0; i < 20; ++i)
{
if(!m_falconDevice.getFalconFirmware()->loadFirmware(skip_checksum, NOVINT_FALCON_NVENT_FIRMWARE_SIZE, const_cast<uint8_t*>(NOVINT_FALCON_NVENT_FIRMWARE)))
{
cout << "Firmware loading try failed" <<endl;
}
else
{
firmware_loaded = true;
break;
}
}
}
}
else if(!firmware_loaded)
{
cout << "No firmware loaded to device, and no firmware specified to load (--nvent_firmware, --test_firmware, etc...). Cannot continue" << endl;
return false;
}
if(!firmware_loaded || !m_falconDevice.isFirmwareLoaded())
{
cout << "No firmware loaded to device, cannot continue" << endl;
return false;
}
cout << "Firmware loaded" << endl;
m_falconDevice.getFalconFirmware()->setHomingMode(true); //Set homing mode (keep track of encoders !needed!)
cout << "Homing Set" << endl;
std::array<int, 3> forces;
//m_falconDevice.getFalconFirmware()->setForces(forces);
m_falconDevice.runIOLoop(); //read in data
bool stop = false;
bool homing = false;
bool homing_reset = false;
usleep(100000);
int tryLoad = 0;
while(!stop) //&& tryLoad < 100)
{
if(!m_falconDevice.runIOLoop()) continue;
if(!m_falconDevice.getFalconFirmware()->isHomed())
{
if(!homing)
{
m_falconDevice.getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
cout << "Falcon not currently homed. Move control all the way out then push straight all the way in." << endl;
}
homing = true;
}
if(homing && m_falconDevice.getFalconFirmware()->isHomed())
{
m_falconDevice.getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::BLUE_LED);
cout << "Falcon homed." << endl;
homing_reset = true;
stop = true;
}
tryLoad++;
}
/*if(tryLoad >= 100)
{
return false;
}*/
m_falconDevice.runIOLoop();
return true;
}
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;
}
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();
}
//Move to setpoint with PID control.
bool runPID()
{
std::array<int, 3> encPos;
/////////////////////////////////////////////
//Request the current encoder positions:
encPos = m_falconDevice.getFalconFirmware()->getEncoderValues();
//Convert encoder positions to leg angles
for(int i = 0; i < 3; i++)
{
Pos[i] = falcon_legs.getTheta(encPos[i]);
}
ROS_DEBUG("Position = %f %f %f",Pos[0], Pos[1], Pos[2]);
//DETERMINE CURRENT ERROR, DELTA ERROR AND INTEGRAL
for(int i = 0; i < 3; i++)
{
Error[i] = SetPoint[i] - Pos[i];
dError[i] = Error[i] - prevError[i];
integral[i] += Error[i];
}
ROS_DEBUG("Error 1 = %f Error 2 = %f Error 3 = %f",Error[0], Error[1], Error[2]);
ROS_DEBUG("dError 1 = %f dError 2 = %f dError 3 = %f",dError[0], dError[1], dError[2]);
ROS_DEBUG("Integral 1 = %f Integral 2 = %f Integral 3 = %f",integral[0], integral[1], integral[2]);
std::array<int, 3> force;
//Simple PID controller. Repeated for each motor.
for(int i = 0; i < 3; i++)
{
force[i]= KpGain[i]*Error[i] + KiGain[i]*integral[i] + KdGain[i]*dError[i];
//Motors saturate at +-4096
if(force[i] > 4096)
force[i] = 4096;
else if(force[i] < -4096)
force[i] = -4096;
}
// force[0] = 0;
// force[1] = 0;
// force[2] = 0;
ROS_DEBUG("Force= %d %d %d",force[0], force[1], force[2]);
m_falconDevice.getFalconFirmware()->setForces(force); //Write falcon forces to driver (Forces updated on IO loop) Should run @ ~2kHz
prevError = Error; //Store current error for comparison on next loop
//IF CURRENT ERROR AND DELTA ERROR < SOME THRESHOLD, INDICATE DONE
if(fabs(Error[0])<=ErrorThreshold && fabs(Error[1])<=ErrorThreshold && fabs(Error[2])<=ErrorThreshold && fabs(dError[0])<=dErrorThreshold && fabs(dError[1])<=dErrorThreshold && fabs(dError[2])<=dErrorThreshold)
{
//Return movement finished
ROS_DEBUG("Movement finished");
return true;
}
else
{
//Return still moving
return false;
}
}