Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
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();
}
Exemplo n.º 5
0
//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;
			}

}