phJoystickControl.cpp

Go to the documentation of this file.

#include <phJoystickControl.h>
#include <phGravisGamePadProData.h>
#include <phSideWinderPrecisionProData.h>
#include <phPlayerUtil.h>
#include <phbase.h>

/* ------------------------------------------------------------------------ */
phJoystickControl::phJoystickControl( phLinuxJoystick   *joystick,
                                      phPlayerPosition  *position,
                                      phPlayerSonar     *sonar,
                                      phPlayerPtz       *ptz
                                    )
{
    phFUNCTION("phJoystickControl::phJoystickControl")
    int locked = 0;
    
    phTHIS_LOOSE_LOCK(locked);

    this->setName("phJoystickControl");
    this->m_joystick    = joystick;
    this->m_sonar       = sonar;
    this->m_position    = position;
    this->m_ptz         = ptz;
    
    phTHIS_LOOSE_UNLOCK(locked);
}

/* ------------------------------------------------------------------------ */
int phJoystickControl::run( )
{
    phFUNCTION("phJoystickControl::run")
    
    int     lock_position   = 0;
    int     position_locked = 0;
    double  translate       = MAX_TRANSLATE;
    double  rotate          = 0.0;
    int     exit_now        = 0;

    int     enabled         = 0;

    int     joy_id          = 0;
    int32_t go      = 0;

    phJoystickData          *joy_data = NULL;

    phCHECK_PTR(this->m_joystick,NULL,"this->m_joystick == NULL");

    if ((strlen(this->m_joystick->getName()) == 0) ||
        (strcmp(this->m_joystick->getName(),
                "Microsoft Microsoft SideWinder Precision Pro (USB)") == 0))
    {              
        phPRINT("Microsoft: %s\n",this->m_joystick->getName());
        
        joy_data = new phSideWinderPrecisionProData();
        joy_id = 1;
    }
    else
    {
        phPRINT("Gravis: %s\n",this->m_joystick->getName());
    
        joy_data = new phGravisGamePadProData();
        joy_id = 0;
    }

    rc = joy_data->connect(this->m_joystick->getSource());
    phCHECK_RC(rc,NULL,"joy_data->connect(this->m_joystick->getSource())");
    
    rc = this->signal_running();
    phCHECK_RC(rc,NULL,"this->signal_running");

    while (this->isRunning() && (exit_now == 0))
    {
        /* Joystick handling */
        rc = joy_data->update(phLiveObjectWAIT);
        phPRINT_RC(rc,NULL,"joy_data->update(WAIT)");

        if (rc == phSUCCESS)
        {
            int32_t red     = 0;
            int32_t green   = 0;
            int32_t blue    = 0;
            int32_t yellow  = 0;
            int32_t x       = 0;
            int32_t y       = 0;
            double rotate_scale = 0.0;
            double translate_scale = 0.0;
            int     home    = 0;
            int32_t pan     = 0;
            int32_t tilt    = 0;
            int32_t zoom    = 0;
    
            x       = joy_data->x();
            y       = joy_data->y();
            red     = joy_data->get("red");
            green   = joy_data->get("green");
            blue    = joy_data->get("blue");
            yellow  = joy_data->get("yellow");

            if (joy_id == 0)
            {
                x           = (red    ? -1 : (green   ? 1 : 0 ));
                y           = (blue   ? -1 : (yellow  ? 1 : 0 ));
                go          = joy_data->get("R1");
                exit_now    = (joy_data->get("start") && joy_data->get("L2")) ? 1 : 0; 

                /* TODO add PTZ */
            }
            else 
            {
                int32_t up          = joy_data->get("up");
                int32_t down        = joy_data->get("down");
                int32_t home_button = joy_data->get("select");
                
                go          = joy_data->get("trigger");
                exit_now    = (joy_data->get("arrow") ? 1 : 0);
                pan         = joy_data->get("hat_roll");
                tilt        = joy_data->get("hat_pitch");

                if ((up && down && home_button) ||
                    (up && down) ||
                    (up && home_button) ||
                    (down && home_button))
                {
                    zoom = home = 0;
                }
                else 
                {
                    if (home_button) home = 1;
                    if (up)
                        zoom = 1;
                    else if (down)
                        zoom = -1;
                }
            }

            //phPROGRESS("%d:%d:%d:%d\n",pan,tilt,home,zoom);
            /* PTZ */
            if ((this->m_ptz != NULL) && 
                (this->m_ptz->isSubscribed()) &&
                ((pan || tilt || home || zoom)))
            {
                double pan_adjust   = 0.5;
                double tilt_adjust  = 0.5;
                double zoom_adjust  = 0.5;

                double pan_val   = 0.0;
                double tilt_val  = 0.0;
                double zoom_val  = 0.0;

                //phPROGRESS("\n");
               
                rc = this->m_ptz->lock();
                phPRINT_RC(rc,NULL,"this->m_ptz->lock()");
               
                if (rc == phSUCCESS)
                {
                    if (home)
                    {
                        pan_val = tilt_val = zoom_val = 0.0;
                    }
                    else
                    {
                        pan_val = this->m_ptz->getPan();
                        tilt_val = this->m_ptz->getTilt();
                        zoom_val = this->m_ptz->getZoom();
                    }        
                    if (pan > 0)        /* pan right */
                        pan_val += pan_adjust;
                    else if (pan < 0)   /* pan left */
                        pan_val += pan_adjust * -1.0;
                    
                    if (tilt < 0)       /* tilt up */
                        tilt_val += tilt_adjust;
                    else if (tilt > 0)  /* tilt down */
                        tilt_val += tilt_adjust * -1.0;
                
                    if (zoom > 0)       /* zoom in */
                        zoom_val += zoom_adjust;
                    else if (zoom < 0)  /* zoom out */
                        zoom_val += zoom_adjust * -1.0;
               
               
                    //phPROGRESS("%0.4f:%0.4f:%0.4f\n",pan_val,tilt_val,zoom_val);
                    rc = this->m_ptz->panTiltZoom(pan_val,tilt_val,zoom_val);
                    phPRINT_RC(rc,NULL,"this->m_ptz->panTiltZoom");
               
                    rc = this->m_ptz->unlock();
                    phPRINT_RC(rc,NULL,"this->m_ptz->unlock()");
                }
            }

            /* MOVEMENT */
            
            rotate_scale    = (double)x / (double)joy_data->valueMax();
            translate_scale = (double)y / (double)joy_data->valueMax();

            //joy_data->print_data();
            
            rotate      = MAX_ROTATE * -1.0 * rotate_scale;
            translate   = MAX_ROTATE * -1.0 * translate_scale;

            /*
            if (go)
            {
                enabled = (enabled == 0 ? 1 : 0);
                phPROGRESS("enabled:%d\n",enabled);
                rc = this->m_position->enable(enabled);
                phPRINT_RC(rc,NULL,"m_position->enable(enabled);");
                if (enabled == 0)
                {
                    this->m_position->stop();
                    phPRINT_RC(rc,NULL,"this->m_position->stop()");
                }
            }
            */
            if (lock_position)
            {
                if ((!position_locked) && (x || y))
                {
                    phPRINT("locked\n");
                    phMUTEX_LOCK((*(this->m_position)),position_locked);
                }
                
                if ((position_locked) && (!x & !y))
                {
                    phPRINT("unlocked\n");
                    phMUTEX_UNLOCK((*(this->m_position)),position_locked);
                }
                
                phPRINT("(joy)enabled:%d trans:%0.8f rot:%0.8f\n",enabled,translate,rotate);
            }
        }

        if (this->m_position->isEnabled() && (!(go > 0)))
        {
            rc = this->m_position->stop();
            phPRINT_RC(rc,NULL,"this->m_position->stop()");
            
            rc = this->m_position->enable(0);
            phPRINT_RC(rc,NULL,"m_position->enable(0);");
        }
        else if ((!(this->m_position->isEnabled())) && (go > 0))
        {
            rc = this->m_position->enable(1);
            phPRINT_RC(rc,NULL,"m_position->enable(1);");
        }

        rc = this->m_position->move(translate,rotate);
        phPRINT_RC(rc,NULL,"move");
    }

    rc = joy_data->disconnect();
    phCHECK_RC(rc,NULL,"joy_data->disconnect()");
                    
    phMUTEX_UNLOCK((*(this->m_position)),position_locked);

    return phSUCCESS;

error:
    rc = this->signal_error();
    phPRINT_RC(rc,NULL,"this->signal_error");

    phMUTEX_ERROR_UNLOCK((*(this->m_position)),position_locked);
    
    return phFAIL;
}

Copyright (C) 2002 - 2007

Philip D.S. Thoren ( pthoren@users.sourceforge.net ) University Of Massachusetts at Lowell Robotics Lab

---