ControllerGlue::~ControllerGlue()
{
if (move_bluetooth != nullptr) {
psmove_disconnect(move_bluetooth);
}
if (move_usb != nullptr) {
psmove_disconnect(move_usb);
}
}
int
main(int argc, char* argv[])
{
PSMove *move;
int i;
int count;
if ((count = psmove_count_connected()) < 1) {
fprintf(stderr, "No controllers connected.\n");
return EXIT_FAILURE;
}
for (i=0; i<count; i++) {
move = psmove_connect_by_id(i);
if (move == NULL) {
fprintf(stderr, "Could not connect to controller #%d.\n", i);
continue;
}
psmove_dump_calibration(move);
psmove_disconnect(move);
}
return EXIT_SUCCESS;
}
int main(int argc, char* argv[])
{
PSMove *move;
if (psmove_count_connected() == 0) {
printf("No controllers connected.\n");
return 2;
}
move = psmove_connect();
if (move == NULL) {
printf("Cannot connect to controller.\n");
return 3;
}
printf("Serial: %s\n", psmove_get_serial(move));
if (psmove_connection_type(move) == Conn_USB) {
_psmove_get_firmware(move);
}
else {
printf("Please connect the controller via USB.\n");
}
psmove_disconnect(move);
return 0;
}
static void
on_monitor_update_moved(enum MonitorEvent event,
enum MonitorEventDeviceType device_type,
const char *path, const wchar_t *serial,
void *user_data)
{
move_daemon *moved = static_cast<move_daemon *>(user_data);
if (event == EVENT_DEVICE_ADDED) {
if (device_type == EVENT_DEVICE_TYPE_USB) {
PSMove *move = psmove_connect_internal((wchar_t*)serial, (char*)path, -1);
if (psmove_pair(move)) {
// Indicate to the user that pairing was successful
psmove_set_leds(move, 0, 255, 0);
psmove_update_leds(move);
} else {
printf("Pairing failed for newly-connected USB device.\n");
}
psmove_disconnect(move);
}
moved->handle_connection(path, serial);
} else if (event == EVENT_DEVICE_REMOVED) {
moved->handle_disconnect(path);
}
}
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;
}
int pair(const char *custom_addr)
{
if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
fprintf(stderr, "PS Move API init failed (wrong version?)\n");
exit(1);
}
int count = psmove_count_connected();
int i;
PSMove *move;
int result = 0;
printf("Connected controllers: %d\n", count);
for (i=0; i<count; i++) {
move = psmove_connect_by_id(i);
if (move == NULL) {
printf("Error connecting to PSMove #%d\n", i+1);
result = 1;
continue;
}
if (psmove_connection_type(move) != Conn_Bluetooth) {
printf("PSMove #%d connected via USB.\n", i+1);
int result = 0;
if (custom_addr != NULL) {
result = psmove_pair_custom(move, custom_addr);
} else {
result = psmove_pair(move);
}
if (result) {
printf("Pairing of #%d succeeded!\n", i+1);
char *serial = psmove_get_serial(move);
printf("Controller address: %s\n", serial);
free(serial);
} else {
printf("Pairing of #%d failed.\n", i+1);
}
if (psmove_has_calibration(move)) {
printf("Calibration data available and saved.\n");
} else {
printf("Error reading/loading calibration data.\n");
}
} else {
printf("Ignoring non-USB PSMove #%d\n", i+1);
}
psmove_disconnect(move);
}
psmove_shutdown();
return result;
}
Tracker::~Tracker() {
psmove_fusion_free(m_fusion);
psmove_tracker_free(m_tracker);
for (int i = 0; i < m_count; i++) {
psmove_disconnect(m_moves[i]);
}
free(m_items);
free(m_moves);
}
PSMoveQt::~PSMoveQt()
{
if (_move != NULL) {
/* Switch off LEDs + rumble on exit */
setColor(Qt::black);
setRumble(0);
psmove_disconnect(_move);
}
}
int main(int arg, char** args) {
int count = psmove_count_connected();
int i;
void *frame;
if (count == 0) {
printf("No controllers connected.\n");
return 1;
}
PSMove **moves = (PSMove **)calloc(count, sizeof(PSMove *));
PSMoveTracker* tracker = psmove_tracker_new();
for (i=0; i<count; i++) {
moves[i] = psmove_connect_by_id(i);
assert(moves[i] != NULL);
while (psmove_tracker_enable(tracker, moves[i])
!= Tracker_CALIBRATED);
}
unsigned char r, g, b;
psmove_tracker_get_camera_color(tracker, moves[0], &r, &g, &b);
printf("Controller color: %02x%02x%02x\n", r, g, b);
CvVideoWriter *writer = cvCreateVideoWriter("out.avi",
CV_FOURCC('M','J','P','G'), 30, cvSize(640, 480), 1);
while ((cvWaitKey(1) & 0xFF) != 27) {
psmove_tracker_update_image(tracker);
psmove_tracker_update(tracker, NULL);
frame = psmove_tracker_get_frame(tracker);
if (frame) {
cvWriteFrame(writer, frame);
}
psmove_tracker_annotate(tracker);
frame = psmove_tracker_get_frame(tracker);
if (frame) {
cvShowImage("live camera feed", frame);
}
}
cvReleaseVideoWriter(&writer);
for (i=0; i<count; i++) {
psmove_disconnect(moves[i]);
}
psmove_tracker_free(tracker);
free(moves);
return 0;
}
void
PSMoveQt::setIndex(int index)
{
if (_index != index) {
_index = index;
PSMove *old = _move;
_move = psmove_connect_by_id(_index);
psmove_disconnect(old);
emit indexChanged();
}
}
void MoveHandler::disconnect()
{
logger->LogEvent("Disconnecting controller.");
cvDestroyAllWindows();
this->frame = nullptr;
for(int i = 0; i < this->connections; i++)
{
psmove_disconnect(this->controllers[i]);
}
psmove_tracker_free(this->tracker);
}
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;
}
int main(int argc, char* argv[])
{
PSMove *move;
int i;
if (psmove_count_connected() == 0) {
printf("No controllers connected.\n");
return 1;
}
move = psmove_connect();
if (move == NULL) {
printf("Cannot connect to controller.\n");
return 2;
}
printf("Connection established.\n");
printf("Serial: %s\n", psmove_get_serial(move));
if (psmove_connection_type(move) != Conn_Bluetooth) {
printf("Controller must be connected via Bluetooth.\n");
return 3;
}
PSMove_Data_AuthChallenge challenge = {
0x02, 0x00, 0x1D, 0x91, 0xE5, 0x81, 0x30, 0x6A,
0x22, 0x9A, 0xAB, 0x2E, 0x80, 0xB4, 0xED, 0x2E,
0xDE, 0x40, 0x0A, 0xF0, 0x02, 0xB0, 0x42, 0x8B,
0x01, 0x41, 0xB2, 0xA4, 0x3D, 0xE7, 0xD4, 0xBF,
0x05, 0x92
};
challenge[0] = 0;
send_and_receive(move, &challenge);
challenge[0] = 1;
send_and_receive(move, &challenge);
for (i = 1; i < 0x10; i++) {
challenge[0] = 0;
challenge[1] = (unsigned char)i;
send_and_receive(move, &challenge);
}
memset(challenge, 0, sizeof(challenge));
send_and_receive(move, &challenge);
psmove_disconnect(move);
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);
}
int main( void )
{
// get number of attached controllers
int count = psmove_count_connected();
if( count < 1 )
{
printf( "No controller connected.\n" );
return 0;
}
printf( "Number of connected controllers: %d\n", count );
// connect to the controller
PSMove* move = psmove_connect();
if( ! move )
{
printf( "Cannot connect to controller.\n" );
return 0;
}
// print the connection type
printf( "Connection type: " );
switch( psmove_connection_type( move ) )
{
case Conn_Bluetooth:
printf( "Bluetooth" );
break;
case Conn_USB:
printf( "USB" );
break;
default:
printf( "Unknown" );
}
printf( "\n" );
// send and receive some different Feature Reports
test_send_auth( move );
test_send_led( move, 255, 255, 0 );
test_read_auth( move );
psmove_disconnect( move );
return 0;
}
int main(int argc, char* argv[])
{
PSMove *move;
int i, c;
if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
fprintf(stderr, "PS Move API init failed (wrong version?)\n");
exit(1);
}
c = psmove_count_connected();
printf("Connected controllers: %d\n", c);
for (i=0; i<c; i++) {
move = psmove_connect_by_id(i);
switch (i) {
case 0:
psmove_set_leds(move, 255, 0, 0);
break;
case 1:
psmove_set_leds(move, 0, 255, 0);
break;
case 2:
psmove_set_leds(move, 0, 0, 255);
break;
case 3:
psmove_set_leds(move, 255, 255, 0);
break;
case 4:
psmove_set_leds(move, 0, 255, 255);
break;
case 5:
psmove_set_leds(move, 255, 0, 255);
break;
default:
psmove_set_leds(move, 255, 255, 255);
break;
}
psmove_update_leds(move);
psmove_disconnect(move);
}
psmove_shutdown();
return 0;
}
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;
}
int main(int argc, char* argv[])
{
PSMove *move = psmove_connect();
int i;
if (move == NULL) {
printf("Could not connect to default Move controller.\n"
"Please connect one via USB or Bluetooth.\n");
exit(1);
}
INFO("Rate limiting is disabled by default");
test_succeeds(move);
INFO("Enabling rate limiting");
psmove_set_rate_limiting(move, 1);
BEGIN_TEST("psmove_update_leds ignores updates (rate limiting enabled)");
for (i=0; i<10; i++) {
psmove_set_leds(move, i, i, i);
assert(psmove_update_leds(move) == Update_Ignored);
}
END_TEST();
INFO("Disabling rate limiting");
psmove_set_rate_limiting(move, 0);
BEGIN_TEST("psmove_update_leds ignores updates (unchanged values)");
psmove_set_leds(move, 1, 1, 1);
assert(psmove_update_leds(move) == Update_Success);
for (i=0; i<10; i++) {
assert(psmove_update_leds(move) == Update_Ignored);
}
END_TEST();
test_succeeds(move);
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);
}
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;
}
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;
}
bool FPSMoveWorker::UpdateControllerConnections(
PSMoveTracker *Tracker,
PSMove **PSMoves)
{
bool controllerCountChanged = false;
uint32 currentTime = FPlatformTime::Cycles();
float millisecondsSinceCheck = FPlatformTime::ToMilliseconds(currentTime - this->LastMoveCountCheckTime);
if (millisecondsSinceCheck >= CONTROLLER_COUNT_POLL_INTERVAL)
{
// Update the number
int newcount = psmove_count_connected();
if (this->PSMoveCount != newcount)
{
UE_LOG(LogPSMove, Log, TEXT("PSMove Controllers count changed: %d -> %d."), this->PSMoveCount, newcount);
this->PSMoveCount = newcount;
controllerCountChanged = true;
}
// Refresh the connection and tracking state of every controller entry
for (int psmove_id = 0; psmove_id < FPSMoveWorker::k_max_controllers; psmove_id++)
{
if (psmove_id < this->PSMoveCount)
{
if (PSMoves[psmove_id] == NULL)
{
// The controller should be connected
PSMoves[psmove_id] = psmove_connect_by_id(psmove_id);
if (PSMoves[psmove_id] != NULL)
{
psmove_enable_orientation(PSMoves[psmove_id], PSMove_True);
assert(psmove_has_orientation(PSMoves[psmove_id]));
this->WorkerControllerDataArray[psmove_id].IsConnected = true;
}
else
{
this->WorkerControllerDataArray[psmove_id].IsConnected = false;
UE_LOG(LogPSMove, Error, TEXT("Failed to connect to PSMove controller %d"), psmove_id);
}
}
if (PSMoves[psmove_id] != NULL && this->WorkerControllerDataArray[psmove_id].IsCalibrated == false)
{
PSMoveTracker_Status tracking_status = psmove_tracker_enable(Tracker, PSMoves[psmove_id]);
psmove_tracker_set_auto_update_leds(Tracker, PSMoves[psmove_id], PSMove_True);
if (tracking_status == Tracker_CALIBRATED)
{
this->WorkerControllerDataArray[psmove_id].IsCalibrated = true;
}
else
{
UE_LOG(LogPSMove, Error, TEXT("Failed to enable tracking for PSMove controller %d (result status: %d)"), psmove_id, (int32)tracking_status);
}
}
}
else
{
// The controller should no longer be tracked
if (PSMoves[psmove_id] != NULL)
{
psmove_disconnect(PSMoves[psmove_id]);
PSMoves[psmove_id] = NULL;
this->WorkerControllerDataArray[psmove_id].IsTracked = false;
this->WorkerControllerDataArray[psmove_id].IsCalibrated = false;
this->WorkerControllerDataArray[psmove_id].IsConnected = false;
}
}
}
// Remember the last time we polled the move count
this->LastMoveCountCheckTime = currentTime;
}
return controllerCountChanged;
}
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(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 arg, char** args) {
int i;
int count = psmove_count_connected();
PSMove* controllers[count];
printf("%s", "### Trying to init PSMoveTracker...");
PSMoveTracker* tracker = psmove_tracker_new();
printf("%s\n", "OK");
printf("### Found %d controllers.\n", count);
void *frame;
unsigned char r, g, b;
int result;
for (i=0; i<count; i++) {
printf("Opening controller %d\n", i);
controllers[i] = psmove_connect_by_id(i);
assert(controllers[i] != NULL);
while (1) {
printf("Calibrating controller %d...", i);
fflush(stdout);
result = psmove_tracker_enable(tracker, controllers[i]);
if (result == Tracker_CALIBRATED) {
printf("OK\n");
break;
} else {
printf("ERROR - retrying\n");
}
}
}
while ((cvWaitKey(1) & 0xFF) != 27) {
psmove_tracker_update_image(tracker);
psmove_tracker_update(tracker, NULL);
frame = psmove_tracker_get_image(tracker);
if (frame) {
cvShowImage("live camera feed", frame);
}
for (i=0; i<count; i++) {
psmove_tracker_get_color(tracker, controllers[i], &r, &g, &b);
psmove_set_leds(controllers[i], r, g, b);
psmove_update_leds(controllers[i]);
float x, y, r;
psmove_tracker_get_position(tracker, controllers[i], &x, &y, &r);
printf("x: %10.2f, y: %10.2f, r: %10.2f\n", x, y, r);
}
}
for (i=0; i<count; i++) {
psmove_disconnect(controllers[i]);
}
psmove_tracker_free(tracker);
return 0;
}
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;
}
//-- 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;
}
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;
}
MainWindow::~MainWindow()
{
psmove_tracker_free(m_tracker);
psmove_disconnect(m_move);
}
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;
}
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;
}
