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;
}
void
Tracker::render()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
PSMoveTrackerRGBImage image = psmove_tracker_get_image(m_tracker);
glEnable(GL_TEXTURE_2D);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image.width, image.height,
0, GL_RGB, GL_UNSIGNED_BYTE, image.data);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/* Draw the camera image, filling the screen */
glColor3f(1., 1., 1.);
glBegin(GL_QUADS);
glTexCoord2f(0., 1.);
glVertex2f(-1., -1.);
glTexCoord2f(1., 1.);
glVertex2f(1., -1.);
glTexCoord2f(1., 0.);
glVertex2f(1., 1.);
glTexCoord2f(0., 0.);
glVertex2f(-1., 1.);
glEnd();
glDisable(GL_TEXTURE_2D);
/* Clear the depth buffer to allow overdraw */
glClear(GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadMatrixf(psmove_fusion_get_projection_matrix(m_fusion));
/* Render the trace */
if (m_trace.size()) {
Point3D center = m_trace[0];
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(center.x + m_offset.x, center.y + m_offset.y, center.z + m_offset.z);
glRotatef(m_rotation, 0., 1., 0.);
std::vector<Point3D>::iterator it;
glColor3f(1., 0., 0.);
glEnable(GL_LIGHTING);
//glBegin(GL_TRIANGLE_STRIP);
for (it=m_trace.begin(); it != m_trace.end(); ++it) {
Point3D point = *it;
Point3D moved(point.x - center.x,
point.y - center.y,
point.z - center.z);
//glVertex3f(moved.x, moved.y, moved.z);
glPushMatrix();
glTranslatef(moved.x, moved.y, moved.z);
glutSolidCube(.5);
glPopMatrix();
}
//glEnd();
glDisable(GL_LIGHTING);
}
for (int i=0; i<m_count; i++) {
glMatrixMode(GL_MODELVIEW);
glLoadMatrixf(psmove_fusion_get_modelview_matrix(m_fusion, m_moves[i]));
if (m_items[i] == WIRE_CUBE) {
glColor3f(1., 0., 0.);
glutWireCube(1.);
glColor3f(0., 1., 0.);
glPushMatrix();
glScalef(1., 1., 4.5);
glTranslatef(0., 0., -.5);
glutWireCube(1.);
glPopMatrix();
glColor3f(0., 0., 1.);
glutWireCube(3.);
} else if (m_items[i] == SOLID_CUBE) {
glEnable(GL_LIGHTING);
glutSolidCube(2.);
glDisable(GL_LIGHTING);
} else if (m_items[i] == SOLID_TEAPOT) {
glEnable(GL_LIGHTING);
glPushMatrix();
glRotatef(90., 1., 0., 0.);
glutSolidTeapot(1.);
glPopMatrix();
glDisable(GL_LIGHTING);
}
}
}
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;
}
