Ejemplo n.º 1
0
void* psmove_updater(void* unused)
{
  while (!game.finished) {
    int i;
    struct psmove_report report;
    
    psmove_poll(psmove, &report);
    
    for (i=0; i < report.device_reports; i++) {
      int j;
      struct psmove_device_report *dr = &report.device_report[i];
      short new_jostle;
      int is_new = 0;
      int new_buttons, pushed_buttons, released_buttons;
      
      /* find our record of which controller this is */
      for (j=0; j < game.controllers && memcmp(&game.controller[j].addr, &dr->addr, sizeof(bdaddr_t)) != 0; j++)
        ;
      if (j == game.controllers) {
        /* new report */
        if (j == 7)
          fatal("too many controllers");
        memcpy(&game.controller[j].addr, &dr->addr, sizeof(bdaddr_t));
        game.controllers++;
        is_new = 1;
      }

      /* rough distance */
      new_jostle = (int) sqrt(
        (psmove_report_get(dr, gx0)*psmove_report_get(dr, gx0)) +
        (psmove_report_get(dr, gy0)*psmove_report_get(dr, gy0)) +
        (psmove_report_get(dr, gz0)*psmove_report_get(dr, gz0))
      ) + (int) sqrt(
        (psmove_report_get(dr, gx1)*psmove_report_get(dr, gx1)) +
        (psmove_report_get(dr, gy1)*psmove_report_get(dr, gy1)) +
        (psmove_report_get(dr, gz1)*psmove_report_get(dr, gz1))
      );
      
      new_jostle /= 2;
      
      game.controller[j].jostle   = new_jostle;
      game.controller[j].jostle1 += new_jostle - 800;
      if (game.controller[j].jostle1 < 0)
        game.controller[j].jostle1 = 0;
      if (game.controller[j].jostle1 > 25600)
        game.controller[j].jostle1 = 25600;
      
      new_buttons = psmove_report_get(dr, buttons);
      pushed_buttons   = (game.controller[j].buttons ^ new_buttons) & new_buttons;
      released_buttons = (game.controller[j].buttons ^ new_buttons) & game.controller[j].buttons;
      game.controller[j].buttons = new_buttons; 
      
      update(is_new, j, pushed_buttons, released_buttons);
        
      //printf("%012li %s: %d %d %x %d\n", micros(), myba2str(&dr->addr), game.controller[j].jostle, game.controller[j].jostle1, game.controller[j].buttons, psmove_report_get(dr, trigger));
    }
  }
  
  return NULL;
}
Ejemplo n.º 2
0
void MoveHandler::updateControllers()
{
	for(int i = 0; i < connections; i++)
	{
		psmove_poll(this->controllers[i]);
		this->buttons[i] = psmove_get_buttons(this->controllers[i]);
	}
}
Ejemplo n.º 3
0
void test_read_performance(PSMove *move, enum TestMode mode) {
    int round, max_rounds = 3;
    float sum = 0.;

    /* Only enable rate limiting if we test for smart updates */
    psmove_set_rate_limiting(move, mode == SMART_UPDATES);

    if (mode == STATIC_UPDATES) {
        psmove_set_leds(move, 0, 255, 0);
    }

    for (round=0; round<max_rounds; round++) {
        long packet_loss = 0;
        long started = psmove_util_get_ticks();
        long reads = 0;
        int old_sequence = -1, sequence;

        while (reads < 1000) {
            if (mode != NO_UPDATES) {
                if (mode != STATIC_UPDATES) {
                    psmove_set_leds(move, reads%255, reads%255, reads%255);
                }

                psmove_update_leds(move);
            }

            if (reads % 20 == 0) {
                fprintf(stderr, "\r%c", "-\\|/"[(reads/20)%4]);
            }

            while (!(sequence = psmove_poll(move))) {}

            if (old_sequence != -1) {
                if (sequence != ((old_sequence % 16) + 1)) {
                    packet_loss++;
                    /*fprintf(stderr, " %d->%d", old_sequence, sequence);*/
                    fputc('x', stderr);
                }
            }
            old_sequence = sequence;

            reads++;
        }
        fputc('\r', stderr);
        long diff = psmove_util_get_ticks() - started;

        double reads_per_second = 1000. * (double)reads / (double)diff;
        printf("%ld reads in %ld ms = %f reads/sec "
                "(%ldx seq jump = %.2f %%)\n",
                reads, diff, reads_per_second, packet_loss,
                100. * (double)packet_loss / (double)reads);
        sum += reads_per_second;
    }

    printf("=====\n");
    printf("Mean over %d rounds: %f reads/sec\n", max_rounds,
            sum/(double)max_rounds);
}
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
void Tracker::update() {
    for (int i = 0; i < m_count; i++) {
        while (psmove_poll(m_moves[i]))
            ;

        float x, y, z;
        psmove_fusion_get_position(m_fusion, m_moves[i], &x, &y, &z);

        int buttons = psmove_get_buttons(m_moves[i]);
        if (buttons & Btn_MOVE) {
            psmove_reset_orientation(m_moves[i]);
        }
        else if (buttons & Btn_PS) {
            exit(0);
        }
        else if (buttons & Btn_SELECT) {
            m_rotation += 2.;
        }
        else if (buttons & Btn_CROSS) {
            m_trace.push_back(Point3D(x, y, z));
        }

        if (buttons & Btn_START) {
            if (m_has_last_offset) {
                m_offset = Point3D(m_offset.x + x - m_last_offset.x,
                                   m_offset.y + y - m_last_offset.y,
                                   m_offset.z + z - m_last_offset.z);
            }
            else {
                m_has_last_offset = true;
            }
            m_last_offset = Point3D(x, y, z);
        }
        else {
            m_has_last_offset = false;
        }

        unsigned int pressed, released;
        psmove_get_button_events(m_moves[i], &pressed, &released);
        if (pressed & Btn_SQUARE) {
            m_items[i] -= 1;
            if (m_items[i] < 0) m_items[i] = ITEM_MAX - 1;
        }
        else if (pressed & Btn_TRIANGLE) {
            m_items[i] += 1;
            if (m_items[i] == ITEM_MAX) m_items[i] = 0;
        }
        else if (pressed & Btn_CIRCLE) {
            m_trace.clear();
            m_rotation = 0.;
            m_offset   = Point3D(0., 0., 0.);
        }
    }

    psmove_tracker_update_image(m_tracker);
    psmove_tracker_update(m_tracker, NULL);
}
Ejemplo n.º 6
0
void test_read_performance(PSMove *move, enum TestMode mode) {
    int round, max_rounds = 5;
    float sum = 0.;

    /* Only enable rate limiting if we test for smart updates */
    psmove_set_rate_limiting(move, mode == SMART_UPDATES);

    if (mode == STATIC_UPDATES) {
        psmove_set_leds(move, 0, 255, 0);
    }

    for (round=0; round<max_rounds; round++) {
        long packet_loss = 0;
        PSMove_timestamp ts_started = psmove_timestamp();
        long reads = 0;
        int old_sequence = -1, sequence;

        while (reads < ITERATIONS) {
            if (mode != NO_UPDATES) {
                if (mode != STATIC_UPDATES) {
                    psmove_set_leds(move, reads%255, reads%255, reads%255);
                }

                psmove_update_leds(move);
            }

            while ((sequence = psmove_poll(move)) == 0);

            if ((old_sequence > 0) && ((old_sequence % 16) != (sequence - 1))) {
                packet_loss++;
            }
            old_sequence = sequence;

            reads++;
        }
        PSMove_timestamp ts_finished = psmove_timestamp();
        float diff = (float)psmove_timestamp_value(psmove_timestamp_diff(ts_finished, ts_started));

        float reads_per_second = (float)reads / diff;
        printf("%ld reads in %.2f ms = %.5f reads/sec "
                "(%ldx seq jump = %.2f %%)\n",
                reads, diff*1000., reads_per_second, packet_loss,
                100. * (double)packet_loss / (double)reads);
        sum += reads_per_second;

        fprintf(csv, "%s,%.10f,%ld,%ld\n",
                testmode_names[mode],
                diff,
                reads,
                packet_loss);
    }

    printf("=====\n");
    printf("Mean over %d rounds: %f reads/sec\n", max_rounds,
            sum/(double)max_rounds);
}
Ejemplo n.º 7
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.º 8
0
        void run()
        {
            PSMove *move;

            /* 6 values = ax, ay, az, gx, gy, gz (accel + gyro) */
            float gx, gy, gz;

            int i = 0;
            qreal rate = 1.;

            assert((move = psmove_connect()) != NULL);
            assert(psmove_connection_type(move) == Conn_Bluetooth);

            assert(psmove_has_calibration(move));

            while (true) {
                if (psmove_poll(move)) {
                    psmove_get_gyroscope_frame(move, Frame_SecondHalf, &gx, &gy, &gz);

                    /**
                     * Playback rate is calculated based on the RPM of the
                     * controller on the Z axis and the normal
                     * playback rate of the turntable (see above).
                     **/
                    rate = RAD_PER_S_TO_RPM(gz) / TURNTABLE_RPM;

                    /* Rate-limiting the updates of the playback rate */
                    if (i % READ_FRAME_UPDATE_RATE == 0) {
                        /**
                         * On Linux, the rate must not be negative. It might be
                         * possible that on Mac OS X or Windows, the rate can
                         * also be negative (depends on the backend).
                         **/
                        if (rate > 0 && rate != player->playbackRate()) {
                            qint64 pos = player->position();
                            player->setPlaybackRate(rate);
                            player->setPosition(pos);
                        }
                    }

                    i++;
                }
            }

            psmove_disconnect(move);
        }
Ejemplo n.º 9
0
void MainWindow::timeout()
{
    unsigned char r, g, b;
    psmove_tracker_get_color(m_tracker, m_move, &r, &g, &b);
    psmove_set_leds(m_move, r, g, b);
    psmove_update_leds(m_move);

    psmove_tracker_update_image(m_tracker);
    psmove_tracker_update(m_tracker, m_move);

    float x, y;
    if (psmove_tracker_get_position(m_tracker, m_move, &x, &y, NULL)) {
        QPointF currentPos(x, y);
        unsigned int buttons = 0;
        while (psmove_poll(m_move)) {
            unsigned int pressed;
            psmove_get_button_events(m_move, &pressed, NULL);
            if (pressed & Btn_MOVE) {
                m_points[m_pointsOffset] = currentPos;
                m_pointsOffset = (m_pointsOffset + 1) % 4;
                m_mapping.set(m_points);
            }
            if (pressed & Btn_T) {
                m_path.moveTo(m_mapping.map(currentPos));
            }
            if (pressed & Btn_CIRCLE) {
                m_path = QPainterPath();
            }
            if (pressed & Btn_PS) {
                QApplication::quit();
            }
            buttons = psmove_get_buttons(m_move);
        }
        if (buttons & Btn_T) {
            m_path.lineTo(m_mapping.map(currentPos));
        }

        m_mousePos = QPointF(x, y);

        update();
    }

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

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

    move = psmove_connect();

    if (move == NULL) {
        fprintf(stderr, "Could not connect to controller.\n");
        return EXIT_FAILURE;
    }

    assert(psmove_has_calibration(move));

    if (psmove_connection_type(move) == Conn_Bluetooth) {
        float ax, ay, az, gx, gy, gz;

        while (1) {
            int res = psmove_poll(move);
            if (res) {
                psmove_get_accelerometer_frame(move, Frame_SecondHalf,
                        &ax, &ay, &az);
                psmove_get_gyroscope_frame(move, Frame_SecondHalf,
                        &gx, &gy, &gz);

                printf("A: %5.2f %5.2f %5.2f ", ax, ay, az);
                printf("G: %6.2f %6.2f %6.2f ", gx, gy, gz);
                printf("\n");
            }
        }
    }

    psmove_disconnect(move);
	psmove_shutdown();

    return EXIT_SUCCESS;
}
Ejemplo n.º 11
0
void
PSMoveAPI::update()
{
    for (auto &c: controllers) {
        if (c->connected && !c->sent_connect) {
            if (receiver->connect != nullptr) {
                // Send initial connect event
                receiver->connect(&c->controller, user_data);
            }
            c->sent_connect = true;
        }

        if (!c->connected && !c->sent_disconnect) {
            if (receiver->disconnect != nullptr) {
                // Send disconnect event
                receiver->disconnect(&c->controller, user_data);
            }
            c->sent_disconnect = true;
        }

        if (!c->connected) {
            // Done with this controller (TODO: Can we check reconnect?)
            continue;
        }

        auto read_move = c->read_move();
        if (read_move == nullptr) {
            // We don't have a handle that supports reading (USB-only connection);
            // we call update exactly once (no new data will be reported), so that
            // the update method can change the LED and rumble values
            if (receiver->update != nullptr) {
                receiver->update(&c->controller, user_data);
            }
        } else {
            while (psmove_poll(read_move)) {
                if (receiver->update != nullptr) {
                    int previous = c->controller.buttons;
                    c->controller.buttons = psmove_get_buttons(read_move);
                    c->controller.pressed = c->controller.buttons & ~previous;
                    c->controller.released = previous & ~c->controller.buttons;
                    c->controller.trigger = float(psmove_get_trigger(read_move)) / 255.f;

                    psmove_get_accelerometer_frame(read_move, Frame_SecondHalf,
                            &c->controller.accelerometer.x,
                            &c->controller.accelerometer.y,
                            &c->controller.accelerometer.z);
                    psmove_get_gyroscope_frame(read_move, Frame_SecondHalf,
                            &c->controller.gyroscope.x,
                            &c->controller.gyroscope.y,
                            &c->controller.gyroscope.z);
                    psmove_get_magnetometer_vector(read_move,
                            &c->controller.magnetometer.x,
                            &c->controller.magnetometer.y,
                            &c->controller.magnetometer.z);
                    c->controller.battery = psmove_get_battery(read_move);

                    receiver->update(&c->controller, user_data);
                }
            }
        }

        auto write_move = c->write_move();
        if (write_move != nullptr) {
            psmove_set_leds(write_move,
                    uint32_t(255 * c->controller.color.r),
                    uint32_t(255 * c->controller.color.g),
                    uint32_t(255 * c->controller.color.b));
            psmove_set_rumble(write_move, uint32_t(255 * c->controller.rumble));
            if (psmove_update_leds(write_move) == Update_Failed) {
                if (read_move != nullptr && write_move != read_move) {
                    // Disconnected from USB, but we read from Bluetooth, so we
                    // can just give up the USB connection and use Bluetooth
                    psmove_disconnect(c->move_usb), c->move_usb = nullptr;
                    // Now that USB is gone, clear the controller's USB flag
                    c->controller.usb = 0;
                } else {
                    c->connected = false;
                }
            }
        }
    }
}
Ejemplo n.º 12
0
int main(int argc, char* argv[])
{
    PSMove *move;
    enum PSMove_Connection_Type ctype;
    int i;

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

    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);
    }

    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;
    }


    if (ctype == Conn_USB) {
        PSMove_Data_BTAddr addr;
        psmove_read_btaddrs(move, &addr, NULL);
        printf("Current BT Host: ");
        for (i=0; i<6; i++) {
            printf("%02x ", addr[i]);
        }
        printf("\n");

#if 0
        /* This is the easy method (pair to this host): */
        if (psmove_pair(move)) {
            printf("Paired. Press the PS Button now :)\n");
        } else {
            printf("psmove_pair() failed :/\n");
        }

        /* This is the advanced method: */

        /* Example BT Address: 01:23:45:67:89:ab */
        const PSMove_Data_BTAddr newhost = {
            0x01, 0x23, 0x45, 0x67, 0x89, 0xab,
        };
        if (!psmove_set_btaddr(move, &newhost)) {
            printf("Could not set BT address!\n");
        }
#endif
    }

    for (i=0; i<10; i++) {
        psmove_set_leds(move, 0, 255*(i%3==0), 0);
        psmove_set_rumble(move, 255*(i%2));
        psmove_update_leds(move);
        usleep(10000*(i%10));
    }

    for (i=250; i>=0; i-=5) {
        psmove_set_leds(move, i, i, 0);
        psmove_set_rumble(move, 0);
        psmove_update_leds(move);
    }

    /* Enable rate limiting for LED updates */
    psmove_set_rate_limiting(move, 1);

    psmove_set_leds(move, 0, 0, 0);
    psmove_set_rumble(move, 0);
    psmove_update_leds(move);

    while (!(psmove_get_buttons(move) & Btn_PS)) {
        int res = psmove_poll(move);
        if (res) {
            if (psmove_get_buttons(move) & Btn_TRIANGLE) {
                printf("Triangle pressed, with trigger value: %d\n",
                        psmove_get_trigger(move));
                psmove_set_rumble(move, psmove_get_trigger(move));
            } else {
                psmove_set_rumble(move, 0x00);
            }

            psmove_set_leds(move, 0, 0, psmove_get_trigger(move));

            int x, y, z;
            psmove_get_accelerometer(move, &x, &y, &z);
            printf("accel: %5d %5d %5d\n", x, y, z);
            psmove_get_gyroscope(move, &x, &y, &z);
            printf("gyro: %5d %5d %5d\n", x, y, z);
            psmove_get_magnetometer(move, &x, &y, &z);
            printf("magnetometer: %5d %5d %5d\n", x, y, z);
            printf("buttons: %x\n", psmove_get_buttons(move));

            int battery = psmove_get_battery(move);

            if (battery == Batt_CHARGING) {
                printf("battery charging\n");
            } else if (battery >= Batt_MIN && battery <= Batt_MAX) {
                printf("battery level: %d / %d\n", battery, Batt_MAX);
            } else {
                printf("battery level: unknown (%x)\n", battery);
            }

            printf("temperature: %d\n", psmove_get_temperature(move));

            psmove_update_leds(move);
        }
    }

    psmove_disconnect(move);

    return 0;
}
Ejemplo n.º 13
0
uint32 FPSMoveWorker::Run()
{
    if (!psmove_init(PSMOVE_CURRENT_VERSION))
    {
        UE_LOG(LogPSMove, Error, TEXT("PS Move API init failed (wrong version?)"));
        return -1;
    }
    // I want the psmoves and psmove_tracker to be local variables in the thread.
    
    // Initialize an empty array of psmove controllers
	PSMove* psmoves[FPSMoveWorker::k_max_controllers];
	memset(psmoves, 0, sizeof(psmoves));

    // Initialize and configure the psmove_tracker
    PSMoveTracker *psmove_tracker = psmove_tracker_new(); // Unfortunately the API does not have a way to change the resolution and framerate.
    PSMoveFusion *psmove_fusion = psmove_fusion_new(psmove_tracker, 1., 1000.);
    int tracker_width = 640;
    int tracker_height = 480;
    if (psmove_tracker)
    {
        UE_LOG(LogPSMove, Log, TEXT("PSMove tracker initialized."));
        
        //Set exposure. TODO: Expose this to component.
        psmove_tracker_set_exposure(psmove_tracker, Exposure_LOW);  //Exposure_LOW, Exposure_MEDIUM, Exposure_HIGH
        psmove_tracker_set_smoothing(psmove_tracker, 0, 1);
		psmove_tracker_set_mirror(psmove_tracker, PSMove_True);
        
        psmove_tracker_get_size(psmove_tracker, &tracker_width, &tracker_height);
        UE_LOG(LogPSMove, Log, TEXT("Camera Dimensions: %d x %d"), tracker_width, tracker_height);
    }
    else {
        UE_LOG(LogPSMove, Log, TEXT("PSMove tracker failed to initialize."));
    }
    
    //Initial wait before starting.
    FPlatformProcess::Sleep(0.03);

    float xcm, ycm, zcm, oriw, orix, oriy, oriz;
    while (StopTaskCounter.GetValue() == 0)
    {        
        // Get positional data from tracker
        if (psmove_tracker)
        {            
			// Setup or tear down controller connections based on the number of active controllers
			UpdateControllerConnections(psmove_tracker, psmoves);

			// Renew the image on camera
			if (PSMoveCount > 0)
			{
				psmove_tracker_update_image(psmove_tracker); // Sometimes libusb crashes here.
				psmove_tracker_update_cbb(psmove_tracker, NULL); // Passing null (instead of m_moves[i]) updates all controllers.
			}
		}
		else {
			FPlatformProcess::Sleep(0.001);
		}

		for (int i = 0; i < PSMoveCount; i++)
		{
			FPSMoveRawControllerData_TLS &localControllerData = WorkerControllerDataArray[i];

			//--------------
			// Read the published data from the component
			//--------------
			localControllerData.WorkerRead();

			// Get positional data from tracker
			if (psmove_tracker)
            {
                localControllerData.IsTracked = psmove_tracker_get_status(psmove_tracker, psmoves[i]) == Tracker_TRACKING;

                psmove_fusion_get_transformed_location(psmove_fusion, psmoves[i], &xcm, &ycm, &zcm);

                if (localControllerData.IsTracked &&
                    xcm && ycm && zcm &&
                    !isnan(xcm) && !isnan(ycm) && !isnan(zcm) &&
                    xcm == xcm && ycm == ycm && zcm == zcm)
                {
                    localControllerData.PosX = xcm;
                    localControllerData.PosY = ycm;
                    localControllerData.PosZ = zcm;
                }
                else {
                    localControllerData.IsTracked = false;
                }

                //UE_LOG(LogPSMove, Log, TEXT("X: %f, Y: %f, Z: %f"), xcm, ycm, zcm);
                if (localControllerData.ResetPoseRequest && localControllerData.IsTracked)
                {
                    psmove_tracker_reset_location(psmove_tracker, psmoves[i]);
                }

                // If we are to change the tracked colour.
                if (localControllerData.UpdateLedRequest)
                {
                    // Stop tracking the controller with the existing color
                    psmove_tracker_disable(psmove_tracker, psmoves[i]);
                    localControllerData.IsTracked = false;
                    localControllerData.IsCalibrated = false;

                    psmove_tracker_set_dimming(psmove_tracker, 0.0);  // Set dimming to 0 to trigger blinking calibration.
                    psmove_set_leds(psmoves[i], 0, 0, 0);  // Turn off the LED to make sure it isn't trackable until new colour set.
                    psmove_update_leds(psmoves[i]);
                    FColor newFColor = localControllerData.LedColourRequest.Quantize();

                    if (psmove_tracker_enable_with_color(psmove_tracker, psmoves[i], newFColor.R, newFColor.G, newFColor.B) == Tracker_CALIBRATED)
                    {
                        this->WorkerControllerDataArray[i].IsCalibrated = true;
                    }
                    else
                    {
                        UE_LOG(LogPSMove, Error, TEXT("Failed to change tracking color for PSMove controller %d"), i);
                    }

                    localControllerData.LedColourWasUpdated = true;
                }
                else
                {
                    localControllerData.LedColourWasUpdated = false;
                }

			}
			else {
				FPlatformProcess::Sleep(0.001);
			}

			// Do bluetooth IO: Orientation, Buttons, Rumble
			
            //TODO: Is it necessary to keep polling until no frames are left?
            while (psmove_poll(psmoves[i]) > 0)
            {
                // Update the controller status (via bluetooth)
                psmove_poll(psmoves[i]);

                // Get the controller orientation (uses IMU).
                psmove_get_orientation(psmoves[i],
                    &oriw, &orix, &oriy, &oriz);
                localControllerData.OriW = oriw;
                localControllerData.OriX = orix;
                localControllerData.OriY = oriy;
                localControllerData.OriZ = oriz;
                //UE_LOG(LogPSMove, Log, TEXT("Ori w,x,y,z: %f, %f, %f, %f"), oriw, orix, oriy, oriz);

                // Get the controller button state
                localControllerData.Buttons = psmove_get_buttons(psmoves[i]);  // Bitwise; tells if each button is down.
                psmove_get_button_events(psmoves[i], &localControllerData.Pressed, &localControllerData.Released);  // i.e., state change

                // Get the controller trigger value (uint8; 0-255)
                localControllerData.TriggerValue = psmove_get_trigger(psmoves[i]);

                // Set the controller rumble (uint8; 0-255)
                psmove_set_rumble(psmoves[i], localControllerData.RumbleRequest);
            }

            if (localControllerData.ResetPoseRequest)
            {
                psmove_reset_orientation(psmoves[i]);
                //TODO: reset yaw only
                localControllerData.PoseWasReset = true;
            }
            else {
                localControllerData.PoseWasReset = false;
            }


			//--------------
			// Publish the updated worker data to the component
			//--------------
			localControllerData.WorkerPost();
        }    

        //Sleeping the thread seems to crash libusb.
        //FPlatformProcess::Sleep(0.005);        
    }
    
    // Delete the controllers
    for (int i = 0; i<PSMoveCount; i++)
    {
        psmove_disconnect(psmoves[i]);
    }

    // Delete the fusion
    if (psmove_fusion)
    {
        psmove_fusion_free(psmove_fusion);
    }
    
    // Delete the tracker
    if (psmove_tracker)
    {
        psmove_tracker_free(psmove_tracker);
    }

    psmove_shutdown();

    return 0;
}
Ejemplo n.º 14
0
void
PSMoveQt::onTimeout()
{
    int ax, ay, az, gx, gy, gz, mx, my, mz, buttons, battery;

    while (psmove_poll(_move)) {
        setTrigger(psmove_get_trigger(_move));
        psmove_get_gyroscope(_move, &gx, &gy, &gz);
        if (gx != _gx || gy != _gy || gz != _gz) {
            _gx = gx;
            _gy = gy;
            _gz = gz;
            emit gyroChanged();
        }
        psmove_get_accelerometer(_move, &ax, &ay, &az);
        if (ax != _ax || ay != _ay || az != _az) {
            _ax = ax;
            _ay = ay;
            _az = az;
            emit accelerometerChanged();
        }

        psmove_get_magnetometer(_move, &mx, &my, &mz);
        if (mx != _mx || my != _my || mz != _mz) {
            _mx = mx;
            _my = my;
            _mz = mz;
            emit magnetometerChanged();
        }

        buttons = psmove_get_buttons(_move);
        if (buttons != _buttons) {
            unsigned int pressed, released;
            psmove_get_button_events(_move, &pressed, &released);

            for (int i=1; i<=PSMoveQt::T; i <<= 1) {
                if (pressed & i) {
                    emit buttonPressed(i);
                } else if (released & i) {
                    emit buttonReleased(i);
                }
            }

            _buttons = buttons;
        }

        battery = psmove_get_battery(_move);
        if (battery != _battery) {
            bool charging = (battery == Batt_CHARGING ||
                    _battery == Batt_CHARGING);

            _battery = battery;

            if (charging) {
                emit chargingChanged();
            } else if (_battery != Batt_CHARGING) {
                emit batteryChanged(_battery);
            }
        }
    }
}
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
int main(int argc, char* argv[])
{
    PSMove *move;
    enum PSMove_Connection_Type ctype;
    int i;

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

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

    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);
    printf("Serial: %s\n", serial);
    free(serial);

    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;
    }

    for (i=0; i<10; i++) {
        psmove_set_leds(move, 0, 255*(i%3==0), 0);
        psmove_set_rumble(move, 255*(i%2));
        psmove_update_leds(move);
        psmove_usleep(10000 * (i % 10));
    }

    for (i=250; i>=0; i-=5) {
        psmove_set_leds(move, i, i, 0);
        psmove_set_rumble(move, 0);
        psmove_update_leds(move);
    }

    /* Enable rate limiting for LED updates */
    psmove_set_rate_limiting(move, 1);

    psmove_set_leds(move, 0, 0, 0);
    psmove_set_rumble(move, 0);
    psmove_update_leds(move);

    while (ctype != Conn_USB && !(psmove_get_buttons(move) & Btn_PS)) {
        int res = psmove_poll(move);
        if (res) {
            if (psmove_get_buttons(move) & Btn_TRIANGLE) {
                printf("Triangle pressed, with trigger value: %d\n",
                        psmove_get_trigger(move));
                psmove_set_rumble(move, psmove_get_trigger(move));
            } else {
                psmove_set_rumble(move, 0x00);
            }

            psmove_set_leds(move, 0, 0, psmove_get_trigger(move));

            int x, y, z;
            psmove_get_accelerometer(move, &x, &y, &z);
            printf("accel: %5d %5d %5d\n", x, y, z);
            psmove_get_gyroscope(move, &x, &y, &z);
            printf("gyro: %5d %5d %5d\n", x, y, z);
            psmove_get_magnetometer(move, &x, &y, &z);
            printf("magnetometer: %5d %5d %5d\n", x, y, z);
            printf("buttons: %x\n", psmove_get_buttons(move));

            int battery = psmove_get_battery(move);

            if (battery == Batt_CHARGING) {
                printf("battery charging\n");
            } else if (battery == Batt_CHARGING_DONE) {
                printf("battery fully charged (on charger)\n");
            } else if (battery >= Batt_MIN && battery <= Batt_MAX) {
                printf("battery level: %d / %d\n", battery, Batt_MAX);
            } else {
                printf("battery level: unknown (%x)\n", battery);
            }

            printf("raw temperature: %d\n", psmove_get_temperature(move));
            printf("celsius temperature: %f\n", psmove_get_temperature_in_celsius(move));

            psmove_update_leds(move);
        }
    }

    psmove_disconnect(move);
    psmove_shutdown();
    return 0;
}
Ejemplo n.º 17
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);
    }

    /* check if at least one controller is attached */
    if (psmove_count_connected() < 1) {
        printf("No Move controller attached.\n");
        exit(1);
    }

    PSMove *move = psmove_connect();
    if(move == NULL) {
        printf("Could not connect to default Move controller.\n");
        exit(1);
    }

    /* make sure we are connected via Bluetooth */
    if (psmove_connection_type(move) != Conn_Bluetooth) {
        printf("Controller must be connected via Bluetooth.\n");
        psmove_disconnect(move);
        exit(1);
    }

    int ext_connected = 0;
    enum Extension_Device ext_device = Ext_Unknown;

    while (!(psmove_get_buttons(move) & Btn_PS)) {
        if (!psmove_poll(move)) {
            continue;
        }

        if(psmove_is_ext_connected(move)) {
            /* if the extension device was not connected before, report connect */
            if (!ext_connected) {
                PSMove_Ext_Device_Info ext;
                enum PSMove_Bool success = psmove_get_ext_device_info(move, &ext);

                if (success) {
                    switch (ext.dev_id) {
                        case EXT_DEVICE_ID_SHARP_SHOOTER:
                            ext_device = Ext_Sharp_Shooter;
                            printf("Sharp Shooter extension connected!\n");
                            break;
                        case EXT_DEVICE_ID_RACING_WHEEL:
                            ext_device = Ext_Racing_Wheel;
                            printf("Racing Wheel extension connected!\n");
                            break;
                        default:
                            ext_device = Ext_Unknown;
                            printf("Unknown extension device (id 0x%04X) connected!\n", ext.dev_id);
                            break;
                    }
                }
                else {
                    printf("Unknown extension device connected! Failed to get device info.\n");
                }
            }

            ext_connected = 1;

            /* handle button presses etc. for a known extension device only */
            PSMove_Ext_Data data;
            if (psmove_get_ext_data(move, &data)) {
                unsigned int move_buttons = psmove_get_buttons(move);

                switch (ext_device) {
                    case Ext_Sharp_Shooter:
                        handle_sharp_shooter(&data, move_buttons);
                        break;
                    case Ext_Racing_Wheel:
                        handle_racing_wheel(move, &data, move_buttons);
                        break;
                    default:
                        break;
                }
            }
        } else {
            /* if the extension device was connected before, report disconnect */
            if (ext_connected) {
                printf("Extension device disconnected!\n");
            }

            ext_connected = 0;
        }
    }

    psmove_disconnect(move);

    return 0;
}
Ejemplo n.º 18
0
int
main(int arg, char** args)
{
    measurement measurements[MEASUREMENTS];
    float distance = MEASUREMENTS_CM_START;
    int pos = 0;

    PSMove *move = psmove_connect();

    if (move == NULL) {
        printf("Could not connect to the default controller.\n");
        return 1;
    }

    PSMoveTracker* tracker = psmove_tracker_new();

    if (tracker == NULL) {
        printf("Could not create tracker.\n");
        return 2;
    }

    printf("Calibrating controller...\n");
    while (psmove_tracker_enable(tracker, move) != Tracker_CALIBRATED);

    while (cvWaitKey(1) != 27 && pos < MEASUREMENTS) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);
        printf("Distance: %.2f cm\n", distance);

        void *frame = psmove_tracker_get_image(tracker);
        cvShowImage("Camera", frame);

        unsigned char r, g, b;
        psmove_tracker_get_color(tracker, move, &r, &g, &b);
        psmove_set_leds(move, r, g, b);
        psmove_update_leds(move);

        float x, y, radius;
        psmove_tracker_get_position(tracker, move, &x, &y, &radius);

        unsigned int pressed, released;
        while (psmove_poll(move));
        psmove_get_button_events(move, &pressed, &released);
        if (pressed & Btn_CROSS) {
            // Save current measurement
            save(frame, (int)distance);
            measurements[pos].distance_cm = distance;
            measurements[pos].radius_px = radius;
            distance += MEASUREMENTS_CM_STEP;
            pos++;
        } else if (pressed & Btn_CIRCLE && pos > 0) {
            // Go back and retry previous measurement
            distance -= MEASUREMENTS_CM_STEP;
            pos--;
        }
    }

    int i;
    FILE *fp = fopen("distance.csv", "w");
    fprintf(fp, "distance,radius\n");
    for (i=0; i<pos; i++) {
        fprintf(fp, "%.5f,%.5f\n",
                measurements[i].distance_cm,
                measurements[i].radius_px);
    }
    fclose(fp);

    psmove_tracker_free(tracker);
    psmove_disconnect(move);

    return 0;
}
Ejemplo n.º 19
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);
    }

    /* Check if at least one controller is attached */
    if (psmove_count_connected() < 1) {
        printf("No Move controller attached.\n");
        exit(1);
    }

    PSMove *move = psmove_connect();
    if(move == NULL) {
        printf("Could not connect to default Move controller.\n");
        exit(1);
    }

    /* Make sure we are connected via Bluetooth */
    if (psmove_connection_type(move) != Conn_Bluetooth) {
        printf("Controller must be connected via Bluetooth.\n");
        psmove_disconnect(move);
        exit(1);
    }

    unsigned int freq_idx = 0;

    while (!(psmove_get_buttons(move) & Btn_PS)) {
        if (!psmove_poll(move)) {
            continue;
        }

        unsigned int pressed_buttons;
        psmove_get_button_events(move, &pressed_buttons, NULL);

        unsigned long frequency = freqs[freq_idx];

        /* Cycle through the list of frequencies */
        if (pressed_buttons & Btn_CROSS) {
            freq_idx = (freq_idx + 1) % NUM_FREQS;
            frequency = freqs[freq_idx];
            printf("Selecting frequency %lu Hz. Press SQUARE to apply.\n", frequency);
        }

        /* Apply the currently selected frequency as new PWM frequency */
        if (pressed_buttons & Btn_SQUARE) {
            printf("Setting LED PWM frequency to %lu Hz.\n", frequency);

            /*
               Switch off the LEDs. If we do not do this, changing the PWM
               frequency will switch off the LEDs and keep them off until
               the Bluetooth connection has been teared down (at least
               once) and then reestablished.
            */
            psmove_set_leds(move, 0, 0, 0);
            psmove_update_leds(move);

            /*
               TODO:
               How can we make sure the LEDs are really off before changing
               the PWM frequency? It seems to work in all tests so far, but
               a proper verification would be nice.
            */
            if (!psmove_set_led_pwm_frequency(move, frequency)) {
                printf("Failed to set LED PWM frequency.\n");
            }
        }

        /* Keep fixed color for visual feedback */
        psmove_set_leds(move, 0, 0, 63);
        psmove_update_leds(move);
    }

    psmove_disconnect(move);
	psmove_shutdown();

    return 0;
}
Ejemplo n.º 20
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;
}