MainWindow::MainWindow(QWidget *parent) :
QWidget(parent),
m_mousePos(0, 0),
m_mapping(),
m_points(),
m_pointsOffset(0),
m_move(psmove_connect()),
m_tracker(psmove_tracker_new()),
m_timer(),
m_path(),
m_rect(rect()),
m_rectOffset(0)
{
m_points[0] = QPointF(1, 1);
m_points[1] = QPointF(640, 0);
m_points[2] = QPointF(640, 480);
m_points[3] = QPointF(0, 480);
m_mapping.set(m_points);
setMouseTracking(true);
if (m_move) {
while (psmove_tracker_enable(m_tracker, m_move) != Tracker_CALIBRATED);
}
QObject::connect(&m_timer, SIGNAL(timeout()),
this, SLOT(timeout()));
m_timer.start(1);
}
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;
}
Tracker::Tracker() : m_moves(NULL), m_count(psmove_count_connected()), mocapRecorder("psmovedata") {
// PSMove *move;
// 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);
// auto 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;
// }
std::cout << "PSMOVE : Initialising Tracker..." << std::endl;
m_tracker = psmove_tracker_new();
std::cout << "PSMOVE : Tracker Initialised" << std::endl;
std::cout << "PSMOVE : Initialising Fusion..." << std::endl;
m_fusion = psmove_fusion_new(m_tracker, 1., 1000.);
std::cout << "PSMOVE : Fusion Initialised" << std::endl;
psmove_tracker_set_mirror(m_tracker, PSMove_False);
psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);
m_moves = (PSMove**) calloc(m_count, sizeof(PSMove*));
m_items = (int*) calloc(m_count, sizeof(int));
for (int i = 0; i < m_count; i++) {
std::cout << "PSMOVE : Initialising Controller " << i << "..." << std::endl;
m_moves[i] = psmove_connect_by_id(i);
m_items[i] = WIRE_CUBE;
psmove_enable_orientation(m_moves[i], PSMove_True);
assert(psmove_has_orientation(m_moves[i]));
std::cout << "PSMOVE : Calibrating Controller " << i << "..." << std::endl;
while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED)
;
std::cout << "PSMOVE : Controller Calibrated " << i << "..." << std::endl;
}
// psmove_tracker_set_dimming(m_tracker,1);
std::cout << "PSMOVE INITIALISED" << std::endl;
}
bool MoveHandler::connectTracker()
{
std::stringstream string;
logger->LogEvent("Initializeing tracker...");
tracker = psmove_tracker_new();
if(this->tracker == nullptr || this->tracker == NULL)
{
logger->LogEvent("Could not create tracker.\nAborting!");
return false;
}
logger->LogEvent("OK");
for(int i = 0; i < this->connections; i++)
{
this->timer.start();
while (*this->running && this->timer.secondsElapsed() < 60)
{
string.str("");
string << "Calibrating connection #" << i;
logger->LogEvent(string.str());
if (psmove_tracker_enable(this->tracker, this->controllers[i]) == Tracker_CALIBRATED)
{
string.str("");
string << "Enable automatic LED update for connection #" << i;
logger->LogEvent(string.str());
psmove_tracker_set_auto_update_leds(this->tracker, this->controllers[i], PSMove_True);
break;
}
else
{
logger->LogEvent("Caibration failed. Retrying.");
}
}
logger->LogEvent("Calibration finished.");
this->bulbHandler->startTimer();
}
logger->LogEvent("Enable tracker mirroring");
psmove_tracker_set_mirror(this->tracker, PSMove_True);
logger->LogEvent("Controller initialization finished");
return true;
}
Tracker::Tracker()
: m_moves(NULL),
m_count(psmove_count_connected()),
m_tracker(psmove_tracker_new()),
m_fusion(psmove_fusion_new(m_tracker, 1., 1000.))
{
psmove_tracker_set_mirror(m_tracker, PSMove_True);
psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);
m_moves = (PSMove**)calloc(m_count, sizeof(PSMove*));
m_items = (int*)calloc(m_count, sizeof(int));
for (int i=0; i<m_count; i++) {
m_moves[i] = psmove_connect_by_id(i);
m_items[i] = WIRE_CUBE;
psmove_enable_orientation(m_moves[i], PSMove_True);
assert(psmove_has_orientation(m_moves[i]));
while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED);
}
}
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;
}
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;
}
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;
}