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; }
PSMoveAPI::PSMoveAPI(EventReceiver *receiver, void *user_data) : receiver(receiver) , user_data(user_data) , controllers() { std::map<std::string, std::vector<PSMove *>> moves; int n = psmove_count_connected(); for (int i=0; i<n; i++) { PSMove *move = psmove_connect_by_id(i); char *tmp = psmove_get_serial(move); std::string serial(tmp); free(tmp); moves[serial].emplace_back(move); } int i = 0; for (auto &kv: moves) { if (kv.second.size() == 2) { // Have two handles for this controller (USB + Bluetooth) controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], kv.second[1])); } else if (kv.second.size() == 1) { // Have only one handle for this controller controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], nullptr)); } else { // FATAL } } }
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; }
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 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(); } }
psmove_dev::psmove_dev(move_daemon *moved, const char *path, const wchar_t *serial) { if (path != NULL) { move = psmove_connect_internal((wchar_t *)serial, (char *)path, moved->count()); } else { move = psmove_connect_by_id(moved->count()); } assigned_id = moved->get_next_id(); psmove_set_rate_limiting(move, PSMove_False); }
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; }
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; }
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; }
PSMoveQt::PSMoveQt(int index) : _move(psmove_connect_by_id(index)), _timer(), _colorTimer(), _index(index), _trigger(0), _color(Qt::black), _rumble(0), _ax(0), _ay(0), _az(0), _gx(0), _gy(0), _gz(0), _mx(0), _my(0), _mz(0), _buttons(0), _battery(0) { _timer.setInterval(INTERVAL_SENSORS); /** * Re-sending the LED color value every 4 secs should be enough. * For this we need a timer, and when color and/or rumble are * set to a non-zero value, we need to activate it. **/ _colorTimer.setInterval(INTERVAL_RGBLEDS); connect(this, SIGNAL(enabledChanged()), this, SLOT(checkColorTimer())); connect(this, SIGNAL(colorChanged()), this, SLOT(checkColorTimer())); connect(this, SIGNAL(rumbleChanged()), this, SLOT(checkColorTimer())); connect(&_timer, SIGNAL(timeout()), this, SLOT(onTimeout())); connect(&_colorTimer, SIGNAL(timeout()), this, SLOT(onColorTimeout())); }
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) { 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; }
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(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; }
bool MoveHandler::connectControllers() { std::stringstream string; enum PSMove_Connection_Type connectionType; logger->LogEvent("Initializeing controller(s)..."); this->connections = psmove_count_connected(); if(this->connections == 0) { logger->LogEvent("No controllers connected. \nAborting!"); return false; } for(int i = 0; i < this->connections; i++) { PSMove* connection = nullptr; unsigned int buttons = 0; this->controllers.push_back(connection); this->buttons.push_back(buttons); } string << "Move connections: " << this->connections; logger->LogEvent(string.str()); for(int i = 0; i < this->connections; i++) { string.str(""); string << "Connecting to connection #" << i; logger->LogEvent(string.str()); this->controllers[i] = psmove_connect_by_id(i); string.str(""); if(this->controllers[i] == nullptr || this->controllers[i] == NULL) { string << "Could not connect to connection #" << i << "\nAborting!"; return false; } else { string << "Connected to connection #" << i; } logger->LogEvent(string.str()); connectionType = psmove_connection_type(this->controllers[i]); string.str(""); string << "Connection #" << i << " connection type: "; if(connectionType==Conn_Bluetooth) { string << "Bluetooth"; this->BTController = this->controllers[i]; } else { string << "USB"; this->USBController = this->controllers[i]; } logger->LogEvent(string.str()); } logger->LogEvent("Controller initialization finished"); return true; }
//-- 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; }