Ejemplo n.º 1
0
int
main(int argc, char* argv[])
{
    PSMove *move;
    int i;
    int count;

    if ((count = psmove_count_connected()) < 1) {
        fprintf(stderr, "No controllers connected.\n");
        return EXIT_FAILURE;
    }

    for (i=0; i<count; i++) {
        move = psmove_connect_by_id(i);

        if (move == NULL) {
            fprintf(stderr, "Could not connect to controller #%d.\n", i);
            continue;
        }

        psmove_dump_calibration(move);
        psmove_disconnect(move);
    }

    return EXIT_SUCCESS;
}
Ejemplo n.º 2
0
PSMoveAPI::PSMoveAPI(EventReceiver *receiver, void *user_data)
    : receiver(receiver)
    , user_data(user_data)
    , controllers()
{
    std::map<std::string, std::vector<PSMove *>> moves;

    int n = psmove_count_connected();
    for (int i=0; i<n; i++) {
        PSMove *move = psmove_connect_by_id(i);

        char *tmp = psmove_get_serial(move);
        std::string serial(tmp);
        free(tmp);

        moves[serial].emplace_back(move);
    }

    int i = 0;
    for (auto &kv: moves) {
        if (kv.second.size() == 2) {
            // Have two handles for this controller (USB + Bluetooth)
            controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], kv.second[1]));
        } else if (kv.second.size() == 1) {
            // Have only one handle for this controller
            controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], nullptr));
        } else {
            // FATAL
        }
    }
}
Ejemplo n.º 3
0
int pair(const char *custom_addr)
{
	if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
		fprintf(stderr, "PS Move API init failed (wrong version?)\n");
		exit(1);		
	}

    int count = psmove_count_connected();
    int i;
    PSMove *move;
    int result = 0;

    printf("Connected controllers: %d\n", count);

    for (i=0; i<count; i++) {
        move = psmove_connect_by_id(i);

        if (move == NULL) {
            printf("Error connecting to PSMove #%d\n", i+1);
            result = 1;
            continue;
        }

        if (psmove_connection_type(move) != Conn_Bluetooth) {
            printf("PSMove #%d connected via USB.\n", i+1);
            int result = 0;

            if (custom_addr != NULL) {
                result = psmove_pair_custom(move, custom_addr);
            } else {
                result = psmove_pair(move);
            }

            if (result) {
                printf("Pairing of #%d succeeded!\n", i+1);
                char *serial = psmove_get_serial(move);
                printf("Controller address: %s\n", serial);
                free(serial);
            } else {
                printf("Pairing of #%d failed.\n", i+1);
            }

            if (psmove_has_calibration(move)) {
                printf("Calibration data available and saved.\n");
            } else {
                printf("Error reading/loading calibration data.\n");
            }
        } else {
            printf("Ignoring non-USB PSMove #%d\n", i+1);
        }

        psmove_disconnect(move);
    }

	psmove_shutdown();

    return result;
}
Ejemplo n.º 4
0
int main(int argc, char* argv[])
{
    PSMove *moves[MAX_CONTROLLERS];

    int controllers = psmove_count_connected();
    int i;
    int quit = 0;

    for (i=0; i<controllers; i++) {
        moves[i] = psmove_connect_by_id(i);
        if (moves[i] == NULL) {
            fprintf(stderr, "Could not connect to controller #%d.\n", i);
            return EXIT_FAILURE;
        }
    }

    while (!quit) {
        for (i=0; i<controllers; i++) {
            int x, y, z;
            int seq = psmove_poll(moves[i]);

            if (seq) {
                int which;

                if (psmove_get_buttons(moves[i]) & Btn_PS) {
                    quit = 1;
                    break;
                }

                seq -= 1;
                for (which=0; which<2; which++) {
                    printf("%d %s PSMOVE  seq=%-2d",
                            i, psmove_get_serial(moves[i]), seq*2+which);

                    psmove_get_half_frame(moves[i], Sensor_Accelerometer,
                            which, &x, &y, &z);
                    printf(" aX=%-6d aY=%-6d aZ=%-6d", x, y, z);

                    psmove_get_half_frame(moves[i], Sensor_Gyroscope,
                            which, &x, &y, &z);
                    printf(" gX=%-6d gY=%-6d gZ=%-6d", x, y, z);

                    psmove_get_magnetometer(moves[i], &x, &y, &z);
                    /* The y value of the magnetometer is inverted in linmctool */
                    printf(" mX=%-5d mY=%-5d mZ=%-5d", x, -y, z);

                    printf("\n");
                }
            }
        }
    }

    for (i=0; i<controllers; i++) {
        psmove_disconnect(moves[i]);
    }

    return 0;
}
Ejemplo n.º 5
0
int main(int arg, char** args) {
    int count = psmove_count_connected();

    int i;
    void *frame;

    if (count == 0) {
        printf("No controllers connected.\n");
        return 1;
    }
    PSMove **moves = (PSMove **)calloc(count, sizeof(PSMove *));

    PSMoveTracker* tracker = psmove_tracker_new();

    for (i=0; i<count; i++) {
        moves[i] = psmove_connect_by_id(i);
        assert(moves[i] != NULL);

        while (psmove_tracker_enable(tracker, moves[i])
                != Tracker_CALIBRATED);
    }

    unsigned char r, g, b;
    psmove_tracker_get_camera_color(tracker, moves[0], &r, &g, &b);
    printf("Controller color: %02x%02x%02x\n", r, g, b);

    CvVideoWriter *writer = cvCreateVideoWriter("out.avi",
            CV_FOURCC('M','J','P','G'), 30, cvSize(640, 480), 1);

    while ((cvWaitKey(1) & 0xFF) != 27) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);

        frame = psmove_tracker_get_frame(tracker);
        if (frame) {
            cvWriteFrame(writer, frame);
        }

        psmove_tracker_annotate(tracker);
        frame = psmove_tracker_get_frame(tracker);
        if (frame) {
            cvShowImage("live camera feed", frame);
        }
    }

    cvReleaseVideoWriter(&writer);

    for (i=0; i<count; i++) {
        psmove_disconnect(moves[i]);
    }

    psmove_tracker_free(tracker);
    free(moves);
    return 0;
}
Ejemplo n.º 6
0
void
PSMoveQt::setIndex(int index)
{
    if (_index != index) {
        _index = index;
        PSMove *old = _move;
        _move = psmove_connect_by_id(_index);
        psmove_disconnect(old);
        emit indexChanged();
    }
}
Ejemplo n.º 7
0
psmove_dev::psmove_dev(move_daemon *moved, const char *path, const wchar_t *serial)
{
    if (path != NULL) {
        move = psmove_connect_internal((wchar_t *)serial, (char *)path, moved->count());
    } else {
        move = psmove_connect_by_id(moved->count());
    }

    assigned_id = moved->get_next_id();

    psmove_set_rate_limiting(move, PSMove_False);
}
Ejemplo n.º 8
0
int main(int argc, char* argv[])
{
    if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
        fprintf(stderr, "PS Move API init failed (wrong version?)\n");
        exit(1);
    }

    int i, c;
    c = psmove_count_connected();
    printf("Connected controllers: %d\n", c);
    if(c==0)
        return 1;

    PSMove **moves = (PSMove **)malloc(sizeof(PSMove *)*c);

    for(i=0; i<c; i++) {
        moves[i] = psmove_connect_by_id(i);
        psmove_set_rate_limiting(moves[i], 1);
    }

    bool running = true;
    while(running) {
        for(i=0; i<c; i++) {
            while(psmove_poll(moves[i])) {}
            int buttons = psmove_get_buttons(moves[i]);
            int x, y, z, r, g, b;
            psmove_get_accelerometer(moves[i], &x, &y, &z);
            r = convert_accel_to_col(x);
            g = convert_accel_to_col(y);
            b = convert_accel_to_col(z);
            if(buttons & Btn_PS)
                running = false;
            else if(buttons & Btn_MOVE)
                psmove_set_leds(moves[i], 255, 255, 255);
            else
				psmove_set_leds(moves[i], (unsigned char)r, (unsigned char)g, (unsigned char)b);
            clock_t t = clock();
            if(((t/CLOCKS_PER_SEC) % 5) == 0)
                psmove_set_rumble(moves[i], 255);
            else
                psmove_set_rumble(moves[i], 0);
            psmove_update_leds(moves[i]);
        }
    }

    for(i=0; i<c; i++) {
        psmove_disconnect(moves[i]);
    }
    free(moves);

    return 0;
}
Ejemplo n.º 9
0
Tracker::Tracker() : m_moves(NULL), m_count(psmove_count_connected()), mocapRecorder("psmovedata") {
    // PSMove *move;
    // move = psmove_connect();
    // if (move == NULL) {
    //     printf("Could not connect to default Move controller.\n"
    //            "Please connect one via USB or Bluetooth.\n");
    //     exit(1);
    // }
    // char *serial = psmove_get_serial(move);
    // auto ctype = psmove_connection_type(move);
    // switch (ctype) {
    //     case Conn_USB:
    //         printf("Connected via USB.\n");
    //         break;
    //     case Conn_Bluetooth:
    //         printf("Connected via Bluetooth.\n");
    //         break;
    //     case Conn_Unknown:
    //         printf("Unknown connection type.\n");
    //         break;
    // }
    std::cout << "PSMOVE : Initialising Tracker..." << std::endl;
    m_tracker = psmove_tracker_new();
    std::cout << "PSMOVE : Tracker Initialised" << std::endl;

    std::cout << "PSMOVE : Initialising Fusion..." << std::endl;
    m_fusion = psmove_fusion_new(m_tracker, 1., 1000.);
    std::cout << "PSMOVE : Fusion Initialised" << std::endl;

    psmove_tracker_set_mirror(m_tracker, PSMove_False);
    psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);

    m_moves = (PSMove**) calloc(m_count, sizeof(PSMove*));
    m_items = (int*) calloc(m_count, sizeof(int));
    for (int i = 0; i < m_count; i++) {
        std::cout << "PSMOVE : Initialising Controller " << i << "..." << std::endl;
        m_moves[i] = psmove_connect_by_id(i);
        m_items[i] = WIRE_CUBE;

        psmove_enable_orientation(m_moves[i], PSMove_True);
        assert(psmove_has_orientation(m_moves[i]));


        std::cout << "PSMOVE : Calibrating Controller " << i << "..." << std::endl;
        while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED)
            ;
        std::cout << "PSMOVE : Controller Calibrated " << i << "..." << std::endl;
    }
    // psmove_tracker_set_dimming(m_tracker,1);
    std::cout << "PSMOVE INITIALISED" << std::endl;
}
Ejemplo n.º 10
0
int main(int argc, char* argv[])
{
    PSMove *move;
    int i, c;

    if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
        fprintf(stderr, "PS Move API init failed (wrong version?)\n");
        exit(1);
    }

    c = psmove_count_connected();
    printf("Connected controllers: %d\n", c);

    for (i=0; i<c; i++) {
        move = psmove_connect_by_id(i);
        switch (i) {
            case 0:
                psmove_set_leds(move, 255, 0, 0);
                break;
            case 1:
                psmove_set_leds(move, 0, 255, 0);
                break;
            case 2:
                psmove_set_leds(move, 0, 0, 255);
                break;
            case 3:
                psmove_set_leds(move, 255, 255, 0);
                break;
            case 4:
                psmove_set_leds(move, 0, 255, 255);
                break;
            case 5:
                psmove_set_leds(move, 255, 0, 255);
                break;
            default:
                psmove_set_leds(move, 255, 255, 255);
                break;
        }
        psmove_update_leds(move);
        psmove_disconnect(move);
    }
    psmove_shutdown();
    return 0;
}
Ejemplo n.º 11
0
PSMoveQt::PSMoveQt(int index)
    : _move(psmove_connect_by_id(index)),
      _timer(),
      _colorTimer(),
      _index(index),
      _trigger(0),
      _color(Qt::black),
      _rumble(0),
      _ax(0),
      _ay(0),
      _az(0),
      _gx(0),
      _gy(0),
      _gz(0),

      _mx(0),
      _my(0),
      _mz(0),

      _buttons(0),
      _battery(0)
{
    _timer.setInterval(INTERVAL_SENSORS);

    /**
     * Re-sending the LED color value every 4 secs should be enough.
     * For this we need a timer, and when color and/or rumble are
     * set to a non-zero value, we need to activate it.
     **/
    _colorTimer.setInterval(INTERVAL_RGBLEDS);
    connect(this, SIGNAL(enabledChanged()),
            this, SLOT(checkColorTimer()));
    connect(this, SIGNAL(colorChanged()),
            this, SLOT(checkColorTimer()));
    connect(this, SIGNAL(rumbleChanged()),
            this, SLOT(checkColorTimer()));

    connect(&_timer, SIGNAL(timeout()),
            this, SLOT(onTimeout()));
    connect(&_colorTimer, SIGNAL(timeout()),
            this, SLOT(onColorTimeout()));
}
Ejemplo n.º 12
0
Tracker::Tracker()
    : m_moves(NULL),
      m_count(psmove_count_connected()),
      m_tracker(psmove_tracker_new()),
      m_fusion(psmove_fusion_new(m_tracker, 1., 1000.))
{
    psmove_tracker_set_mirror(m_tracker, PSMove_True);
    psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);

    m_moves = (PSMove**)calloc(m_count, sizeof(PSMove*));
    m_items = (int*)calloc(m_count, sizeof(int));
    for (int i=0; i<m_count; i++) {
        m_moves[i] = psmove_connect_by_id(i);
        m_items[i] = WIRE_CUBE;

        psmove_enable_orientation(m_moves[i], PSMove_True);
        assert(psmove_has_orientation(m_moves[i]));

        while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED);
    }
}
Ejemplo n.º 13
0
int main(int arg, char** args) {
    int i;
    int count = psmove_count_connected();
    PSMove* controllers[count];

    printf("%s", "### Trying to init PSMoveTracker...");
    PSMoveTracker* tracker = psmove_tracker_new();
    printf("%s\n", "OK");
    printf("### Found %d controllers.\n", count);

    void *frame;
    unsigned char r, g, b;
    int result;

    for (i=0; i<count; i++) {
        printf("Opening controller %d\n", i);
        controllers[i] = psmove_connect_by_id(i);
        assert(controllers[i] != NULL);

        while (1) {
            printf("Calibrating controller %d...", i);
            fflush(stdout);
            result = psmove_tracker_enable(tracker, controllers[i]);

            if (result == Tracker_CALIBRATED) {
                printf("OK\n");
                break;
            } else {
                printf("ERROR - retrying\n");
            }
        }

    }

    while ((cvWaitKey(1) & 0xFF) != 27) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);

        frame = psmove_tracker_get_image(tracker);
        if (frame) {
            cvShowImage("live camera feed", frame);
        }

        for (i=0; i<count; i++) {
            psmove_tracker_get_color(tracker, controllers[i], &r, &g, &b);
            psmove_set_leds(controllers[i], r, g, b);
            psmove_update_leds(controllers[i]);

            float x, y, r;
            psmove_tracker_get_position(tracker, controllers[i], &x, &y, &r);
            printf("x: %10.2f, y: %10.2f, r: %10.2f\n", x, y, r);
        }
    }

    for (i=0; i<count; i++) {
        psmove_disconnect(controllers[i]);
    }

    psmove_tracker_free(tracker);
    return 0;
}
Ejemplo n.º 14
0
bool FPSMoveWorker::UpdateControllerConnections(
	PSMoveTracker *Tracker,
	PSMove **PSMoves)
{
	bool controllerCountChanged = false;
	uint32 currentTime = FPlatformTime::Cycles();
	float millisecondsSinceCheck = FPlatformTime::ToMilliseconds(currentTime - this->LastMoveCountCheckTime);

	if (millisecondsSinceCheck >= CONTROLLER_COUNT_POLL_INTERVAL)
	{
		// Update the number 
		int newcount = psmove_count_connected();

		if (this->PSMoveCount != newcount)
		{
			UE_LOG(LogPSMove, Log, TEXT("PSMove Controllers count changed: %d -> %d."), this->PSMoveCount, newcount);

			this->PSMoveCount = newcount;
			controllerCountChanged = true;
		}

		// Refresh the connection and tracking state of every controller entry
		for (int psmove_id = 0; psmove_id < FPSMoveWorker::k_max_controllers; psmove_id++)
		{
			if (psmove_id < this->PSMoveCount)
			{
				if (PSMoves[psmove_id] == NULL)
				{
					// The controller should be connected
					PSMoves[psmove_id] = psmove_connect_by_id(psmove_id);

					if (PSMoves[psmove_id] != NULL)
					{
						psmove_enable_orientation(PSMoves[psmove_id], PSMove_True);
						assert(psmove_has_orientation(PSMoves[psmove_id]));

						this->WorkerControllerDataArray[psmove_id].IsConnected = true;
					}
					else
					{
						this->WorkerControllerDataArray[psmove_id].IsConnected = false;
						UE_LOG(LogPSMove, Error, TEXT("Failed to connect to PSMove controller %d"), psmove_id);
					}
				}

				if (PSMoves[psmove_id] != NULL && this->WorkerControllerDataArray[psmove_id].IsCalibrated == false)
				{
					PSMoveTracker_Status tracking_status = psmove_tracker_enable(Tracker, PSMoves[psmove_id]);
					psmove_tracker_set_auto_update_leds(Tracker, PSMoves[psmove_id], PSMove_True);
					
					if (tracking_status == Tracker_CALIBRATED)
					{
						this->WorkerControllerDataArray[psmove_id].IsCalibrated = true;
					}
					else
					{
						UE_LOG(LogPSMove, Error, TEXT("Failed to enable tracking for PSMove controller %d (result status: %d)"), psmove_id, (int32)tracking_status);
					}
				}
			}
			else
			{
				// The controller should no longer be tracked
				if (PSMoves[psmove_id] != NULL)
				{
					psmove_disconnect(PSMoves[psmove_id]);
					PSMoves[psmove_id] = NULL;
					this->WorkerControllerDataArray[psmove_id].IsTracked = false;
					this->WorkerControllerDataArray[psmove_id].IsCalibrated = false;
					this->WorkerControllerDataArray[psmove_id].IsConnected = false;
				}
			}
		}

		// Remember the last time we polled the move count
		this->LastMoveCountCheckTime = currentTime;
	}

	return controllerCountChanged;
}
Ejemplo n.º 15
0
int main(void)
{
    PSMove *moves[7];

    int last_xs[7];
    int last_ys[7];
    int last_zs[7];

    float decay = 0.009;
    float increase = 0.004;
    
    // amount to get to brightness
    float brightness = 0.49;

    int told_everyone = 0;

    int dist_thresh;
    char *env_dist_thresh = getenv("DIST_THRESH");
    if (env_dist_thresh == NULL) {
        dist_thresh = 1000;
    }
    else {
        dist_thresh = atoi(env_dist_thresh);
    }

    int i, j, move_count;

    int rcolors[][3] = {
      {254, 0, 0}, {255, 0, 0}, {255, 7, 0}, {255, 7, 0}, {255, 13, 0},
      {255, 20, 0}, {255, 20, 0}, {255, 26, 0}, {255, 26, 0}, {255, 33, 0},
      {255, 33, 0}, {255, 39, 0}, {255, 39, 0}, {255, 46, 0}, {255, 46, 0},
      {255, 52, 0}, {255, 59, 0}, {255, 59, 0}, {255, 65, 0}, {255, 65, 0},
      {255, 72, 0}, {255, 72, 0}, {255, 78, 0}, {255, 78, 0}, {255, 85, 0},
      {255, 85, 0}, {255, 91, 0}, {255, 97, 0}, {255, 97, 0}, {255, 103, 0},
      {255, 103, 0}, {255, 109, 0}, {255, 109, 0}, {255, 116, 0}, {255, 116, 0},
      {255, 122, 0}, {255, 122, 0}, {255, 128, 0}, {255, 134, 0}, {255, 134, 0},
      {255, 140, 0}, {255, 140, 0}, {255, 146, 0}, {255, 146, 0}, {255, 153, 0},
      {255, 153, 0}, {255, 159, 0}, {255, 159, 0}, {255, 165, 0}, {255, 171, 0},
      {255, 171, 0}, {255, 177, 0}, {255, 177, 0}, {255, 183, 0}, {255, 183, 0},
      {255, 190, 0}, {255, 190, 0}, {255, 196, 0}, {255, 196, 0}, {255, 202, 0},
      {255, 208, 0}, {255, 208, 0}, {255, 213, 0}, {255, 213, 0}, {255, 218, 0},
      {255, 218, 0}, {255, 224, 0}, {255, 224, 0}, {255, 229, 0}, {255, 234, 0},
      {255, 234, 0}, {255, 239, 0}, {255, 239, 0}, {255, 245, 0}, {255, 245, 0},
      {255, 250, 0}, {255, 250, 0}, {255, 255, 0}, {255, 255, 0}, {250, 255, 0},
      {246, 255, 0}, {246, 255, 0}, {241, 255, 0}, {241, 255, 0}, {236, 255, 0},
      {236, 255, 0}, {230, 255, 0}, {230, 255, 0}, {224, 255, 0}, {224, 255, 0},
      {218, 255, 0}, {212, 255, 0}, {212, 255, 0}, {206, 255, 0}, {206, 255, 0},
      {200, 255, 0}, {200, 255, 0}, {194, 255, 0}, {194, 255, 0}, {188, 255, 0},
      {188, 255, 0}, {181, 255, 0}, {174, 255, 0}, {174, 255, 0}, {167, 255, 0},
      {167, 255, 0}, {160, 255, 0}, {160, 255, 0}, {153, 255, 0}, {153, 255, 0},
      {146, 255, 0}, {146, 255, 0}, {139, 255, 0}, {132, 255, 0}, {132, 255, 0},
      {125, 255, 0}, {125, 255, 0}, {118, 255, 0}, {118, 255, 0}, {111, 255, 0},
      {111, 255, 0}, {103, 255, 0}, {103, 255, 0}, {96, 255, 0}, {88, 255, 0},
      {88, 255, 0}, {81, 255, 0}, {81, 255, 0}, {73, 255, 0}, {73, 255, 0},
      {66, 255, 0}, {66, 255, 0}, {58, 255, 0}, {50, 255, 0}, {50, 255, 0},
      {43, 255, 0}, {43, 255, 0}, {35, 255, 0}, {35, 255, 0}, {28, 255, 0},
      {28, 255, 0}, {20, 255, 0}, {20, 255, 0}, {13, 255, 0}, {5, 255, 0},
      {5, 255, 0}, {0, 255, 0}, {0, 255, 0}, {0, 255, 4}, {0, 255, 4},
      {0, 255, 9}, {0, 255, 9}, {0, 255, 13}, {0, 255, 13}, {0, 255, 18},
      {0, 255, 22}, {0, 255, 22}, {0, 255, 27}, {0, 255, 27}, {0, 255, 31},
      {0, 255, 31}, {0, 255, 36}, {0, 255, 36}, {0, 255, 42}, {0, 255, 42},
      {0, 255, 47}, {0, 255, 52}, {0, 255, 52}, {0, 255, 58}, {0, 255, 58},
      {0, 255, 63}, {0, 255, 63}, {0, 255, 68}, {0, 255, 68}, {0, 255, 74},
      {0, 255, 74}, {0, 255, 79}, {0, 255, 84}, {0, 255, 84}, {0, 255, 90},
      {0, 255, 90}, {0, 255, 95}, {0, 255, 95}, {0, 255, 100}, {0, 255, 100},
      {0, 255, 106}, {0, 255, 106}, {0, 255, 111}, {0, 255, 117}, {0, 255, 117},
      {0, 255, 122}, {0, 255, 122}, {0, 255, 128}, {0, 255, 128}, {0, 255, 134},
      {0, 255, 134}, {0, 255, 140}, {0, 255, 145}, {0, 255, 145}, {0, 255, 151},
      {0, 255, 151}, {0, 255, 157}, {0, 255, 157}, {0, 255, 163}, {0, 255, 163},
      {0, 255, 168}, {0, 255, 168}, {0, 255, 174}, {0, 255, 180}, {0, 255, 180},
      {0, 255, 185}, {0, 255, 185}, {0, 255, 191}, {0, 255, 191}, {0, 255, 197},
      {0, 255, 197}, {0, 255, 203}, {0, 255, 203}, {0, 255, 208}, {0, 255, 214},
      {0, 255, 214}, {0, 255, 220}, {0, 255, 220}, {0, 255, 226}, {0, 255, 226},
      {0, 255, 231}, {0, 255, 231}, {0, 255, 237}, {0, 255, 237}, {0, 255, 242},
      {0, 255, 246}, {0, 255, 246}, {0, 255, 251}, {0, 255, 251}, {0, 255, 255},
      {0, 255, 255}, {0, 249, 255}, {0, 249, 255}, {0, 242, 255}, {0, 242, 255},
      {0, 236, 255}, {0, 230, 255}, {0, 230, 255}, {0, 223, 255}, {0, 223, 255},
      {0, 217, 255}, {0, 217, 255}, {0, 211, 255}, {0, 211, 255}, {0, 204, 255},
      {0, 204, 255}, {0, 198, 255}, {0, 192, 255}, {0, 192, 255}, {0, 185, 255},
      {0, 185, 255}, {0, 179, 255}, {0, 179, 255}, {0, 173, 255}, {0, 173, 255},
      {0, 166, 255}, {0, 160, 255}, {0, 160, 255}, {0, 153, 255}, {0, 153, 255},
      {0, 147, 255}, {0, 147, 255}, {0, 140, 255}, {0, 140, 255}, {0, 133, 255},
      {0, 133, 255}, {0, 127, 255}, {0, 120, 255}, {0, 120, 255}, {0, 113, 255},
      {0, 113, 255}, {0, 107, 255}, {0, 107, 255}, {0, 100, 255}, {0, 100, 255},
      {0, 93, 255}, {0, 93, 255}, {0, 87, 255}, {0, 80, 255}, {0, 80, 255},
      {0, 73, 255}, {0, 73, 255}, {0, 67, 255}, {0, 67, 255}, {0, 60, 255},
      {0, 60, 255}, {0, 53, 255}, {0, 53, 255}, {0, 47, 255}, {0, 40, 255},
      {0, 40, 255}, {0, 33, 255}, {0, 33, 255}, {0, 27, 255}, {0, 27, 255},
      {0, 20, 255}, {0, 20, 255}, {0, 13, 255}, {0, 13, 255}, {0, 7, 255},
      {0, 0, 255}, {0, 0, 255}, {6, 0, 255}, {6, 0, 255}, {11, 0, 255},
      {11, 0, 255}, {17, 0, 255}, {17, 0, 255}, {22, 0, 255}, {22, 0, 255},
      {28, 0, 255}, {33, 0, 255}, {33, 0, 255}, {39, 0, 255}, {39, 0, 255},
      {44, 0, 255}, {44, 0, 255}, {50, 0, 255}, {50, 0, 255}, {55, 0, 255},
      {61, 0, 255}, {61, 0, 255}, {67, 0, 255}, {67, 0, 255}, {72, 0, 255},
      {72, 0, 255}, {78, 0, 255}, {78, 0, 255}, {83, 0, 255}, {83, 0, 255},
      {89, 0, 255}, {94, 0, 255}, {94, 0, 255}, {100, 0, 255}, {100, 0, 255},
      {105, 0, 255}, {105, 0, 255}, {111, 0, 255}, {111, 0, 255}, {116, 0, 255},
      {116, 0, 255}, {122, 0, 255}, {127, 0, 255}, {127, 0, 255}, {133, 0, 255},
      {133, 0, 255}, {139, 0, 255}, {139, 0, 255}, {144, 0, 255}, {144, 0, 255},
      {150, 0, 255}, {150, 0, 255}, {155, 0, 255}, {161, 0, 255}, {161, 0, 255},
      {166, 0, 255}, {166, 0, 255}, {172, 0, 255}, {172, 0, 255}, {177, 0, 255},
      {177, 0, 255}, {183, 0, 255}, {183, 0, 255}, {188, 0, 255}, {194, 0, 255},
      {194, 0, 255}, {200, 0, 255}, {200, 0, 255}, {205, 0, 255}, {205, 0, 255},
      {211, 0, 255}, {211, 0, 255}, {216, 0, 255}, {216, 0, 255}, {222, 0, 255},
      {227, 0, 255}, {227, 0, 255}, {233, 0, 255}, {233, 0, 255}, {238, 0, 255},
      {238, 0, 255}, {244, 0, 255}, {244, 0, 255}, {249, 0, 255}, {255, 0, 255},
      {255, 0, 255}, {255, 0, 249}, {255, 0, 249}, {255, 0, 243}, {255, 0, 243},
      {255, 0, 237}, {255, 0, 237}, {255, 0, 230}, {255, 0, 230}, {255, 0, 224},
      {255, 0, 218}, {255, 0, 218}, {255, 0, 212}, {255, 0, 212}, {255, 0, 206},
      {255, 0, 206}, {255, 0, 200}, {255, 0, 200}, {255, 0, 193}, {255, 0, 193},
      {255, 0, 187}, {255, 0, 181}, {255, 0, 181}, {255, 0, 175}, {255, 0, 175},
      {255, 0, 169}, {255, 0, 169}, {255, 0, 163}, {255, 0, 163}, {255, 0, 157},
      {255, 0, 157}, {255, 0, 150}, {255, 0, 144}, {255, 0, 144}, {255, 0, 138},
      {255, 0, 138}, {255, 0, 132}, {255, 0, 132}, {255, 0, 126}, {255, 0, 126},
      {255, 0, 120}, {255, 0, 120}, {255, 0, 113}, {255, 0, 107}, {255, 0, 107},
      {255, 0, 101}, {255, 0, 101}, {255, 0, 95}, {255, 0, 95}, {255, 0, 89},
      {255, 0, 89}, {255, 0, 83}, {255, 0, 83}, {255, 0, 77}, {255, 0, 71},
      {255, 0, 71}, {255, 0, 65}, {255, 0, 65}, {255, 0, 59}, {255, 0, 59},
      {255, 0, 53}, {255, 0, 53}, {255, 0, 48}, {255, 0, 42}, {255, 0, 42},
      {255, 0, 36}, {255, 0, 36}, {255, 0, 30}, {255, 0, 30}, {255, 0, 24},
      {255, 0, 24}, {255, 0, 18}, {255, 0, 18}, {255, 0, 12}, {255, 0, 6},
      {255, 0, 6}, {255, 0, 0}, {255, 0, 0}
    };

    for (i = 7; i >= 0; i--) { 
      last_xs[i] = 0;
      last_ys[i] = 0;
      last_zs[i] = 0;
    }

    move_count = psmove_count_connected();
    printf("Connected controllers: %d\n", move_count);

    if (move_count < 1) {
        printf("You must have at least 1 player!\n");
        exit(1);
    }

    for (i = move_count-1; i >= 0; i--) {
        moves[i] = psmove_connect_by_id(i);
        if (moves[i] == NULL) {
            printf("Could not connect to controller\n");
            exit(1);
        }
        else {
            /* Enable rate limiting for LED updates */
            psmove_set_rate_limiting(moves[i], 1);
        }
    }

    // reset
    for (i = move_count-1; i >= 0; i--) {
        psmove_set_leds(moves[i], 0, 0, 0);
        psmove_set_rumble(moves[i], 0);
        psmove_update_leds(moves[i]);
    }

    int colors[] = {0, 0, 0, 0, 0, 0, 0};
    int trigger_downs[] = {0, 0, 0, 0, 0, 0, 0};
    int max_color = (sizeof(rcolors) / sizeof(int)) / 3;

    while (!(psmove_get_buttons(moves[0]) & Btn_PS)) {
        for (i = move_count-1; i >= 0; i--) {
            PSMove *move = moves[i];
            int res = psmove_poll(move);
            if (res) {
                int x, y, z;
                psmove_get_gyroscope(move, &x, &y, &z);
                int diff_x = x - last_xs[i];
                int diff_y = y - last_ys[i];
                int diff_z = z - last_zs[i];

                // update last pos
                last_xs[i] = x;
                last_ys[i] = y;
                last_zs[i] = z;

                float distance = sqrt((diff_x * diff_x) + (diff_y * diff_y) + (diff_z * diff_z));

                if (distance > dist_thresh) {
                    colors[i] = colors[i] + 1;
                    if (colors[i] >= max_color) { colors[i] = 0; }

                    psmove_set_leds(move, rcolors[colors[i]][0] * brightness, rcolors[colors[i]][1] * brightness, rcolors[colors[i]][2] * brightness);
                    psmove_update_leds(move);
                }

            }
            usleep(10000);
        }
    }

    for (i = move_count-1; i >= 0; i--) {
        psmove_disconnect(moves[i]);
    }

    return 0;
}
Ejemplo n.º 16
0
bool MoveHandler::connectControllers()
{
	std::stringstream string;
	enum PSMove_Connection_Type connectionType;

	logger->LogEvent("Initializeing controller(s)...");
	
	this->connections = psmove_count_connected();

	if(this->connections == 0)
	{
		logger->LogEvent("No controllers connected. \nAborting!");
		return false;
	}
	
	for(int i = 0; i < this->connections; i++)
	{
		PSMove* connection = nullptr;
		unsigned int buttons = 0;

		this->controllers.push_back(connection);
		this->buttons.push_back(buttons);
	}

	string << "Move connections: " << this->connections;
	logger->LogEvent(string.str());

	for(int i = 0; i < this->connections; i++)
	{
		string.str("");
		string << "Connecting to connection #" << i;
		logger->LogEvent(string.str());
		
		this->controllers[i] = psmove_connect_by_id(i);

		string.str("");
		if(this->controllers[i] == nullptr || this->controllers[i] == NULL)
		{
			string << "Could not connect to connection #" << i << "\nAborting!";		
			return false;
		}
		else
		{
			string << "Connected to connection #" << i;
		}
		logger->LogEvent(string.str());
		
		connectionType = psmove_connection_type(this->controllers[i]);
		string.str("");
		string << "Connection #" << i << " connection type: ";
		if(connectionType==Conn_Bluetooth)
		{
			string << "Bluetooth";
			this->BTController = this->controllers[i]; 
		}
		else
		{
			string << "USB";
			this->USBController = this->controllers[i];
		}	
		logger->LogEvent(string.str());
	}

    logger->LogEvent("Controller initialization finished");
    return true;
}
Ejemplo n.º 17
0
//-- public methods ----
int
main(int arg, char** args)
{
	if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
		fprintf(stderr, "PS Move API init failed (wrong version?)\n");
		exit(1);
	}

    int i;
    int count = psmove_count_connected();

    printf("Connected controllers for calibration: %d\n", count);

    for (i=0; i<count; i++) 
	{
        PSMove *move = psmove_connect_by_id(i);

        if (move == NULL)
        {
            printf("Failed to open PS Move #%d\n", i);
            continue;
        }

		// Make sure accelerometer is calibrated
		// Otherwise we can't tell if the controller is sitting upright
		if (psmove_has_calibration(move))
		{
			if (psmove_connection_type(move) == Conn_Bluetooth)
			{
				float old_range = 0;
				enum eCalibrationState calibration_state = Calibration_MeasureBExtents;

				// Reset existing magnetometer calibration state
				psmove_reset_magnetometer_calibration(move);

				printf("Calibrating PS Move #%d\n", i);
				printf("[Step 1 of 2: Measuring extents of the magnetometer]\n");
				printf("Rotate the controller in all directions.\n");
				fflush(stdout);

				while (calibration_state == Calibration_MeasureBExtents)
				{
					if (psmove_poll(move))
					{
						unsigned int pressed, released;
						psmove_get_button_events(move, &pressed, &released);

						/* This call updates min/max values if exceeding previously stored values */
						psmove_get_magnetometer_3axisvector(move, NULL);

						/* Returns the minimum delta (max-min) across dimensions (x, y, z) */
						float range = psmove_get_magnetometer_calibration_range(move);

						/* Update the LEDs to indicate progress */
						int percentage = (int)(100.f * range / LED_RANGE_TARGET);
						if (percentage > 100)
						{
							percentage = 100;
						}
						else if (percentage < 0)
						{
							percentage = 0;
						}

						psmove_set_leds(
							move,
							(unsigned char)((255 * (100 - percentage)) / 100),
							(unsigned char)((255 * percentage) / 100),
							0);
						psmove_update_leds(move);

						if (pressed & Btn_MOVE)
						{
							psmove_set_leds(move, 0, 0, 0);
							psmove_update_leds(move);

							// Move on to the 
							calibration_state = Calibration_WaitForGravityAlignment;
						}
						else if (range > old_range)
						{
							old_range = range;
							printf("\rPress the MOVE button when value stops changing: %.1f", range);
							fflush(stdout);
						}
					}
				}

				printf("\n\n[Step 2 of 2: Measuring default magnetic field direction]\n");
				printf("Stand the controller on a level surface with the Move button facing you.\n");
				printf("This will be the default orientation of the move controller.\n");
				printf("Measurement will start once the controller is aligned with gravity and stable.\n");
				fflush(stdout);

				//TODO: Wait for accelerometer stabilization and button press
				// Sample the magnetometer field for several frames and average
				// Store as the default magnetometer direction
				while (calibration_state != Calibration_Complete)
				{
					int stable_start_time = psmove_util_get_ticks();
					PSMove_3AxisVector identity_pose_average_m_vector = *k_psmove_vector_zero;
					int sample_count = 0;

					while (calibration_state == Calibration_WaitForGravityAlignment)
					{
						if (psmove_poll(move))
						{
							if (is_move_stable_and_aligned_with_gravity(move))
							{
								int current_time = psmove_util_get_ticks();
								int stable_duration = (current_time - stable_start_time);

								if ((current_time - stable_start_time) >= STABILIZE_WAIT_TIME_MS)
								{
									calibration_state = Calibration_MeasureBDirection;
								}
								else
								{
									printf("\rStable for: %dms/%dms                        ", stable_duration, STABILIZE_WAIT_TIME_MS);
								}
							}
							else
							{
								stable_start_time = psmove_util_get_ticks();
								printf("\rMove Destabilized! Waiting for stabilization.");
							}
						}
					}

					printf("\n\nMove stabilized. Starting magnetometer sampling.\n");
					while (calibration_state == Calibration_MeasureBDirection)
					{
						if (psmove_poll(move))
						{
							if (is_move_stable_and_aligned_with_gravity(move))
							{
								PSMove_3AxisVector m;

								psmove_get_magnetometer_3axisvector(move, &m);
								psmove_3axisvector_normalize_with_default(&m, k_psmove_vector_zero);

								identity_pose_average_m_vector = psmove_3axisvector_add(&identity_pose_average_m_vector, &m);
								sample_count++;

								if (sample_count > DESIRED_MAGNETOMETER_SAMPLE_COUNT)
								{
									float N = (float)sample_count;

									// The average magnetometer direction was recorded while the controller
									// was in the cradle pose
									identity_pose_average_m_vector = psmove_3axisvector_divide_by_scalar_unsafe(&identity_pose_average_m_vector, N);

									psmove_set_magnetometer_calibration_direction(move, &identity_pose_average_m_vector);

									calibration_state = Calibration_Complete;
								}
								else
								{
									printf("\rMagnetometer Sample: %d/%d                   ", sample_count, DESIRED_MAGNETOMETER_SAMPLE_COUNT);
								}
							}
							else
							{
								calibration_state = Calibration_WaitForGravityAlignment;
								printf("\rMove Destabilized! Waiting for stabilization.\n");
							}
						}
					}
				}

				psmove_save_magnetometer_calibration(move);
			}
			else
			{
				printf("Ignoring non-Bluetooth PS Move #%d\n", i);
			}
		}
		else
		{			
			char *serial= psmove_get_serial(move);

			printf("\nController #%d has bad calibration file (accelerometer values won't be correct).\n", i);

			if (serial != NULL)
			{
				printf("Please delete %s.calibration and re-run \"psmove pair\" with the controller plugged into usb.", serial);
				free(serial);
			}
			else
			{
				printf("Please re-run \"psmove pair\" with the controller plugged into usb.");
			}
		}

        printf("\nFinished PS Move #%d\n", i);
        psmove_disconnect(move);
    }

    return 0;
}