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
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;
}
int main(int argc, char* argv[])
{
PSMove *move;
if (psmove_count_connected() == 0) {
printf("No controllers connected.\n");
return 2;
}
move = psmove_connect();
if (move == NULL) {
printf("Cannot connect to controller.\n");
return 3;
}
printf("Serial: %s\n", psmove_get_serial(move));
if (psmove_connection_type(move) == Conn_USB) {
_psmove_get_firmware(move);
}
else {
printf("Please connect the controller via USB.\n");
}
psmove_disconnect(move);
return 0;
}
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 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 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;
}
int
main(int argc, char *argv[])
{
move_daemon moved;
#ifdef __linux
moved_monitor *monitor = moved_monitor_new(on_monitor_update_moved, &moved);
#endif
/* Never act as a client in "moved" mode */
psmove_set_remote_config(PSMove_OnlyLocal);
moved.dump_devices();
int id, count = psmove_count_connected();
for (id=0; id<count; id++) {
moved.handle_connection(NULL, NULL);
}
#ifdef __linux
fd_set fds;
int server_fd = moved.get_socket();
int monitor_fd = moved_monitor_get_fd(monitor);
int nfds = ((server_fd > monitor_fd)?(server_fd):(monitor_fd)) + 1;
#endif
for (;;) {
#ifdef __linux
FD_ZERO(&fds);
FD_SET(server_fd, &fds);
FD_SET(monitor_fd, &fds);
if (select(nfds, &fds, NULL, NULL, NULL)) {
if (FD_ISSET(monitor_fd, &fds)) {
moved_monitor_poll(monitor);
}
if (FD_ISSET(server_fd, &fds)) {
moved.handle_request();
}
}
#else
moved.handle_request();
#endif
moved.write_reports();
}
#ifdef __linux
moved_monitor_free(monitor);
#endif
return 0;
}
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;
if (psmove_count_connected() == 0) {
printf("No controllers connected.\n");
return 1;
}
move = psmove_connect();
if (move == NULL) {
printf("Cannot connect to controller.\n");
return 2;
}
printf("Connection established.\n");
printf("Serial: %s\n", psmove_get_serial(move));
if (psmove_connection_type(move) != Conn_Bluetooth) {
printf("Controller must be connected via Bluetooth.\n");
return 3;
}
PSMove_Data_AuthChallenge challenge = {
0x02, 0x00, 0x1D, 0x91, 0xE5, 0x81, 0x30, 0x6A,
0x22, 0x9A, 0xAB, 0x2E, 0x80, 0xB4, 0xED, 0x2E,
0xDE, 0x40, 0x0A, 0xF0, 0x02, 0xB0, 0x42, 0x8B,
0x01, 0x41, 0xB2, 0xA4, 0x3D, 0xE7, 0xD4, 0xBF,
0x05, 0x92
};
challenge[0] = 0;
send_and_receive(move, &challenge);
challenge[0] = 1;
send_and_receive(move, &challenge);
for (i = 1; i < 0x10; i++) {
challenge[0] = 0;
challenge[1] = (unsigned char)i;
send_and_receive(move, &challenge);
}
memset(challenge, 0, sizeof(challenge));
send_and_receive(move, &challenge);
psmove_disconnect(move);
return 0;
}
int main( void )
{
// get number of attached controllers
int count = psmove_count_connected();
if( count < 1 )
{
printf( "No controller connected.\n" );
return 0;
}
printf( "Number of connected controllers: %d\n", count );
// connect to the controller
PSMove* move = psmove_connect();
if( ! move )
{
printf( "Cannot connect to controller.\n" );
return 0;
}
// print the connection type
printf( "Connection type: " );
switch( psmove_connection_type( move ) )
{
case Conn_Bluetooth:
printf( "Bluetooth" );
break;
case Conn_USB:
printf( "USB" );
break;
default:
printf( "Unknown" );
}
printf( "\n" );
// send and receive some different Feature Reports
test_send_auth( move );
test_send_led( move, 255, 255, 0 );
test_read_auth( move );
psmove_disconnect( move );
return 0;
}
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;
}
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 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;
}
int
PSMoveQt::count()
{
return psmove_count_connected();
}
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);
}
int ext_connected = 0;
enum Extension_Device ext_device = Ext_Unknown;
while (!(psmove_get_buttons(move) & Btn_PS)) {
if (!psmove_poll(move)) {
continue;
}
if(psmove_is_ext_connected(move)) {
/* if the extension device was not connected before, report connect */
if (!ext_connected) {
PSMove_Ext_Device_Info ext;
enum PSMove_Bool success = psmove_get_ext_device_info(move, &ext);
if (success) {
switch (ext.dev_id) {
case EXT_DEVICE_ID_SHARP_SHOOTER:
ext_device = Ext_Sharp_Shooter;
printf("Sharp Shooter extension connected!\n");
break;
case EXT_DEVICE_ID_RACING_WHEEL:
ext_device = Ext_Racing_Wheel;
printf("Racing Wheel extension connected!\n");
break;
default:
ext_device = Ext_Unknown;
printf("Unknown extension device (id 0x%04X) connected!\n", ext.dev_id);
break;
}
}
else {
printf("Unknown extension device connected! Failed to get device info.\n");
}
}
ext_connected = 1;
/* handle button presses etc. for a known extension device only */
PSMove_Ext_Data data;
if (psmove_get_ext_data(move, &data)) {
unsigned int move_buttons = psmove_get_buttons(move);
switch (ext_device) {
case Ext_Sharp_Shooter:
handle_sharp_shooter(&data, move_buttons);
break;
case Ext_Racing_Wheel:
handle_racing_wheel(move, &data, move_buttons);
break;
default:
break;
}
}
} else {
/* if the extension device was connected before, report disconnect */
if (ext_connected) {
printf("Extension device disconnected!\n");
}
ext_connected = 0;
}
}
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;
}
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 argc, char* argv[])
{
PSMove *move;
enum PSMove_Connection_Type ctype;
int i;
i = psmove_count_connected();
printf("Connected controllers: %d\n", i);
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);
}
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;
}
if (ctype == Conn_USB) {
PSMove_Data_BTAddr addr;
psmove_read_btaddrs(move, &addr, NULL);
printf("Current BT Host: ");
for (i=0; i<6; i++) {
printf("%02x ", addr[i]);
}
printf("\n");
#if 0
/* This is the easy method (pair to this host): */
if (psmove_pair(move)) {
printf("Paired. Press the PS Button now :)\n");
} else {
printf("psmove_pair() failed :/\n");
}
/* This is the advanced method: */
/* Example BT Address: 01:23:45:67:89:ab */
const PSMove_Data_BTAddr newhost = {
0x01, 0x23, 0x45, 0x67, 0x89, 0xab,
};
if (!psmove_set_btaddr(move, &newhost)) {
printf("Could not set BT address!\n");
}
#endif
}
for (i=0; i<10; i++) {
psmove_set_leds(move, 0, 255*(i%3==0), 0);
psmove_set_rumble(move, 255*(i%2));
psmove_update_leds(move);
usleep(10000*(i%10));
}
for (i=250; i>=0; i-=5) {
psmove_set_leds(move, i, i, 0);
psmove_set_rumble(move, 0);
psmove_update_leds(move);
}
/* Enable rate limiting for LED updates */
psmove_set_rate_limiting(move, 1);
psmove_set_leds(move, 0, 0, 0);
psmove_set_rumble(move, 0);
psmove_update_leds(move);
while (!(psmove_get_buttons(move) & Btn_PS)) {
int res = psmove_poll(move);
if (res) {
if (psmove_get_buttons(move) & Btn_TRIANGLE) {
printf("Triangle pressed, with trigger value: %d\n",
psmove_get_trigger(move));
psmove_set_rumble(move, psmove_get_trigger(move));
} else {
psmove_set_rumble(move, 0x00);
}
psmove_set_leds(move, 0, 0, psmove_get_trigger(move));
int x, y, z;
psmove_get_accelerometer(move, &x, &y, &z);
printf("accel: %5d %5d %5d\n", x, y, z);
psmove_get_gyroscope(move, &x, &y, &z);
printf("gyro: %5d %5d %5d\n", x, y, z);
psmove_get_magnetometer(move, &x, &y, &z);
printf("magnetometer: %5d %5d %5d\n", x, y, z);
printf("buttons: %x\n", psmove_get_buttons(move));
int battery = psmove_get_battery(move);
if (battery == Batt_CHARGING) {
printf("battery charging\n");
} else if (battery >= Batt_MIN && battery <= Batt_MAX) {
printf("battery level: %d / %d\n", battery, Batt_MAX);
} else {
printf("battery level: unknown (%x)\n", battery);
}
printf("temperature: %d\n", psmove_get_temperature(move));
psmove_update_leds(move);
}
}
psmove_disconnect(move);
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;
}
int main(int argc, char* argv[])
{
PSMove *move;
enum PSMove_Connection_Type ctype;
int i;
if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
fprintf(stderr, "PS Move API init failed (wrong version?)\n");
exit(1);
}
i = psmove_count_connected();
printf("Connected controllers: %d\n", i);
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);
printf("Serial: %s\n", serial);
free(serial);
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;
}
for (i=0; i<10; i++) {
psmove_set_leds(move, 0, 255*(i%3==0), 0);
psmove_set_rumble(move, 255*(i%2));
psmove_update_leds(move);
psmove_usleep(10000 * (i % 10));
}
for (i=250; i>=0; i-=5) {
psmove_set_leds(move, i, i, 0);
psmove_set_rumble(move, 0);
psmove_update_leds(move);
}
/* Enable rate limiting for LED updates */
psmove_set_rate_limiting(move, 1);
psmove_set_leds(move, 0, 0, 0);
psmove_set_rumble(move, 0);
psmove_update_leds(move);
while (ctype != Conn_USB && !(psmove_get_buttons(move) & Btn_PS)) {
int res = psmove_poll(move);
if (res) {
if (psmove_get_buttons(move) & Btn_TRIANGLE) {
printf("Triangle pressed, with trigger value: %d\n",
psmove_get_trigger(move));
psmove_set_rumble(move, psmove_get_trigger(move));
} else {
psmove_set_rumble(move, 0x00);
}
psmove_set_leds(move, 0, 0, psmove_get_trigger(move));
int x, y, z;
psmove_get_accelerometer(move, &x, &y, &z);
printf("accel: %5d %5d %5d\n", x, y, z);
psmove_get_gyroscope(move, &x, &y, &z);
printf("gyro: %5d %5d %5d\n", x, y, z);
psmove_get_magnetometer(move, &x, &y, &z);
printf("magnetometer: %5d %5d %5d\n", x, y, z);
printf("buttons: %x\n", psmove_get_buttons(move));
int battery = psmove_get_battery(move);
if (battery == Batt_CHARGING) {
printf("battery charging\n");
} else if (battery == Batt_CHARGING_DONE) {
printf("battery fully charged (on charger)\n");
} else if (battery >= Batt_MIN && battery <= Batt_MAX) {
printf("battery level: %d / %d\n", battery, Batt_MAX);
} else {
printf("battery level: unknown (%x)\n", battery);
}
printf("raw temperature: %d\n", psmove_get_temperature(move));
printf("celsius temperature: %f\n", psmove_get_temperature_in_celsius(move));
psmove_update_leds(move);
}
}
psmove_disconnect(move);
psmove_shutdown();
return 0;
}
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;
}
