/Users/craigcornelius/Projects/SPRING Mac Release 0.2/joystick.cpp

Go to the documentation of this file.

// $Id: joystick.cpp,v 1.8 2006/05/24 16:52:39 sean Exp $
// $Copyright: (c)2001 National Biocomputation Center, Stanford University $
//
//  joystick.cpp
//
//  Author: Chris (caschwan@hawaii.edu) - Hawaii

#include "joystick.h"
#include "glui.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#ifdef _WIN32
#else
#include <string.h>
#endif

const char* Joystick::rcsid = "@(#) $Id: joystick.cpp,v 1.8 2006/05/24 16:52:39 sean Exp $ $Copyright: (c)2001 National Biocomputation Center, Stanford University $";

// buffer size- well under the 1500 bytes ethernet limit, 
// so will fit in 1 packet
#define BUFSIZE 500

// debug level for info messages
int Joystick::debug = 0;



// Joystick constructor
Joystick::Joystick(char* machine_name, int port_num) : Sensor(Sensor::joystick_sensor)
{
        int i;

        // output debug info
    if (debug == 1) {
        cerr << "Joystick::constructor(\"" 
                         << machine_name << "\", "
             << port_num << ")\n";
    }

    // set up socket: (need a separate, non global buffer so each instantiation
    // has it's own messages coming in
    buf = (char *)malloc(sizeof(char)*BUFSIZE);
    char server[80];
    strncpy(server, machine_name, 80);
    
    cerr << "making new socket to: " << server << endl;
    hsock = new Socket(server,Socket::tcp,port_num);
    cerr << "created socket" << endl;
      
        // reset timer
        timer = Timer();
        lastUpdate = 0.0;

        // default joystick emulation mode (x-y like mouse)
        joystickEmulation = Joystick::mouse_emulation;
        scalefactor = 25.0;

        // init joystick data structure
        joydata = new Joydata;
        joydata->num_axes = 0;
        joydata->num_buttons = 0;
        for (i = 0; i < Joystick::MAX_JOYSTICK_BUTTONS; i++) {
                joydata->buttons[i] = 0;
        }
        for (i = 0; i < Joystick::MAX_JOYSTICK_AXES; i++) {
                joydata->axes[i] = 0;
        }

    // // send initial handshake
    sprintf(buf, "joystick v1.0");
#ifdef BUFFERED
    hsock->BufferedSend(buf,strlen(buf));
#else
    hsock->Send(buf,BUFSIZE);
#endif
    cerr << " SEND HANDSHAKE BUF: [" << buf << "]\n";
    
    // Check to make sure haptic responded with an acknowledgement
    // (should ideally respond with what type of device it is, so we can
    // do device-specific things on this side)
#ifdef BUFFERED
    hsock->BufferedReceive(buf,BUFSIZE);
#else
    hsock->Receive(buf,BUFSIZE);
#endif
    cerr << " RECEIVE HANDSHAKE BUF: [" << buf << "]\n";
    
    // test their response- do they support this version of the protocol?
    if (strncmp(buf,"ACK",3) != 0) {
        // no ACK, so an error has occured and we cannot connect
        cerr << "ERROR - denied connection" << endl;
    } else {
        // success
        cerr << "Connection made" << endl;
    }
}

// ends the joystick server
Joystick::~Joystick()
{
        // destructor info
    if (debug == 1) cerr << "Joystick::destructor()\n";
    
    sprintf(buf, "quit");
#ifdef BUFFERED
    hsock->BufferedSend(buf,strlen(buf));
#else
    hsock->Send(buf,BUFSIZE);
#endif
    
    delete hsock;
    free(buf);
    num_active = -1;
}

void Joystick::send_force()
{
    // not supported yet
        // -> SDL (on the joystickserver) only supports status read, but not setting
        //    forces: suggest to wait for future releases of SDL
        // -> see haptic_v1 on how to implement sending forces over the network
}

void Joystick::receive_info()
{
    // Get the joystick data (activation values only)
    int num = sscanf(buf, "%i %i %i %i %i %i %i %i %i %i %i %i %i %i %lf %lf %lf %lf %lf %lf",
         &joydata->num_buttons, 
                 &joydata->buttons[0],  
                 &joydata->buttons[1],  
                 &joydata->buttons[2],  
                 &joydata->buttons[3],  
                 &joydata->buttons[4],  
                 &joydata->buttons[5],  
                 &joydata->buttons[6],  
                 &joydata->buttons[7],  
                 &joydata->buttons[8],  
                 &joydata->buttons[9],  
                 &joydata->buttons[10],  
                 &joydata->buttons[11],  
         &joydata->num_axes,
                 &joydata->axes[0],
                 &joydata->axes[1],
                 &joydata->axes[2],
                 &joydata->axes[3],
                 &joydata->axes[4],
                 &joydata->axes[5]
    );
    
        // debugging
    if (debug == 4) {
        cerr << "Joystick: Received BUF: " << buf << endl;
        }
    else if (debug == 5) {
                cerr << "\tgot " << num << " args from device\n";
    }
}

// update joystick data
void Joystick::Update()
{
    int i;

    // get the buffer
    int data_waiting = hsock->InputQueueSize();

        // if nothing to do, bail
        if (data_waiting > 0) {
                // eat all waiting data to get to last
                while (data_waiting > 0) {
#ifdef BUFFERED
                        int nb = hsock->BufferedReceive(buf, BUFSIZE);
#else
                        int nb = hsock->Receive(buf, BUFSIZE);
#endif
                        data_waiting -= nb;
                }
                // there's data and we got it- process it
                receive_info();
    }

        // do selected emulation
        if (lastUpdate != 0.0) {
                // time diff since last update (seconds that passed)
                double timeDiff = timer.GetTime() - lastUpdate;

                if (joystickEmulation == Joystick::no_emulation) {
                        // NO_EMULATION: save the activation values
                        num_active = joydata->num_buttons + joydata->num_axes;    // calculate from number of arguments seen
                        for (i = 0; i < joydata->num_axes; i++) {
                                active[i] = (joydata->axes[i] + 1) / 2;     // convert from [-1..1] -> [0..1]
                        }
                        for (i = 0; i < joydata->num_buttons; i++) {
                                active[i + joydata->num_axes] = (float)joydata->buttons[i];
                        }
                }
                else if (joystickEmulation == Joystick::mouse_emulation) {
                        // MOUSE_EMULATION
                        num_active = joydata->num_buttons;    // calculate from number of arguments seen
                        for (i = 0; i < joydata->num_buttons; i++) {
                                active[i] = (float)joydata->buttons[i];
                        }

                        if (joydata->num_axes >= 3) {
                                // use axis 0 + 1 for x + y movements
                                pos.x += joydata->axes[0] * scalefactor * timeDiff;
                                pos.y -= joydata->axes[1] * scalefactor * timeDiff;
                                // use axis 2 for z movements
                                pos.z += joydata->axes[2] * scalefactor * timeDiff;
                        }
                }
                else if (joystickEmulation == Joystick::enhanced_3d_emulation) {
                        // ENHANCED_3D_EMULATION
                        num_active = joydata->num_buttons;    // calculate from number of arguments seen
                        for (int i = 0; i < joydata->num_buttons; i++) {
                                active[i] = (float)joydata->buttons[i];
                        }

                        if (joydata->num_axes >= 4) {
                                // calculate new position
                                Point3D posIncrement = Point3D(0, 0, joydata->axes[1] * scalefactor * timeDiff);
                                pos = pos + m.Xform(posIncrement);

                                // camera rotation
                                m.EulerMultiply('z', 60.0 * joydata->axes[0] * timeDiff);
                                m.EulerMultiply('x', 60.0 * joydata->axes[2] * timeDiff);
                                m.EulerMultiply('y', 60.0 * joydata->axes[3] * timeDiff);
                        }
                }
                else if (joystickEmulation == Joystick::first_person_emulation) {
                        // FIRST_PERSON_EMULATION
                        num_active = joydata->num_buttons;    // calculate from number of arguments seen
                        for (int i = 0; i < joydata->num_buttons; i++) {
                                active[i] = (float)joydata->buttons[i];
                        }

                        if (joydata->num_axes >= 4) {
                                // calculate new position
                                Point3D posIncrement = Point3D(0, 0, joydata->axes[1] * scalefactor * timeDiff);
                                pos = pos + m.Xform(posIncrement);
                                posIncrement = Point3D(joydata->axes[0] * scalefactor * timeDiff, 0, 0);
                                pos = pos + m.Xform(posIncrement);

                                // camera rotation
                                m.EulerMultiply('x', 60.0 * joydata->axes[2] * timeDiff);
                                m.EulerMultiply('y', 60.0 * joydata->axes[3] * timeDiff);
                        }
                }
                else if (joystickEmulation == Joystick::pedal_emulation) {
                        // PEDAL_EMULATION (CH Products pedal)
                        num_active = joydata->num_buttons;    // calculate from number of arguments seen
                        for (int i = 0; i < joydata->num_buttons; i++) {
                                active[i] = (float)joydata->buttons[i];
                        }
                        
                        if (joydata->num_axes >= 3) {
                                // calculate new position
                                double zoomMovement = joydata->axes[2];
                                if (zoomMovement >= 0.15) {
                                        zoomMovement -= 0.15;
                                }
                                else if (zoomMovement <= -0.15) {
                                        zoomMovement += 0.15;
                                }
                                else {
                                        zoomMovement = 0.0;
                                }
                                Point3D posIncrement = Point3D(0, 0, -zoomMovement / 2 * scalefactor * timeDiff);
                                pos = pos + m.Xform(posIncrement);

                                // camera rotation
                                if (joydata->axes[1] >= -0.5) {
                                        m.EulerMultiply('x', 20.0 * (joydata->axes[1] + 0.5) / 2 * timeDiff);
                                }
                                else if (joydata->axes[0] >= -0.5) {
                                        m.EulerMultiply('x', -20.0 * (joydata->axes[0] + 0.5) / 2 * timeDiff);
                                }
                        }
                }
                else if (joystickEmulation == Joystick::button_pedal_emulation) {
                        // BUTTON_PEDAL_EMULATION (X-keys pedal)
                        num_active = 3;    
                        for (int i = 0; i < num_active; i++) {
                                active[i] = (float)joydata->buttons[i];
                        }
                        
                        // button mapping X-keys Pedal:
                        // Left=Button0, Center=Button1, Right=Button2
                        float zoomMovement = 0;
                        if (joydata->buttons[0]) {
                                zoomMovement = -0.7;
                        }
                        else if (joydata->buttons[2]) {
                                zoomMovement = 0.7;
                        }

                        if (zoomMovement != 0) {
                                Point3D posIncrement = Point3D(0, 0, -zoomMovement / 2 * scalefactor * timeDiff);
                                pos = pos + m.Xform(posIncrement);
                        }
                }

                // update timer
                lastUpdate = lastUpdate + timeDiff;
        }
        else {
                // reset timer
                lastUpdate = timer.GetTime();
        }

        // and send the force update
    send_force();
}

---