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;
}
void MoveHandler::updateControllers()
{
for(int i = 0; i < connections; i++)
{
psmove_poll(this->controllers[i]);
this->buttons[i] = psmove_get_buttons(this->controllers[i]);
}
}
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);
}
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;
}
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);
}
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);
}
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;
}
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);
}
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();
}
}
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;
}
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;
}
}
}
}
}
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;
}
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;
}
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);
}
}
}
}
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;
}
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;
}
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;
}
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;
}
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;
}
//-- 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;
}