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);
}
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 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();
}
}
void MoveHandler::updateTracker()
{
float xPos = 0.0f;
float yPos = 0.0f;
float ledRadius = 0.0f;
psmove_tracker_update_image(this->tracker);
psmove_tracker_update(this->tracker, NULL);
#if DEBUG
psmove_tracker_annotate(this->tracker);
frame = psmove_tracker_get_frame(this->tracker);
if (frame)
{
cvShowImage("Live camera feed", frame);
}
#endif
psmove_tracker_get_position(this->tracker, this->USBController, &xPos, &yPos, &ledRadius);
this->x = xPos;
this->y = yPos;
this->radius = ledRadius;
}
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 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;
}
