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 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[])
{
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;
}
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);
}
}
}
}
//-- 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;
}
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;
}