int main(int argc, char *argv[])
{
srand(time(0));
while (1) {
// Pick a random tilt and a random LED state
freenect_led_options led = (freenect_led_options) (rand() % 6); // explicit cast
int tilt = (rand() % 30)-15;
freenect_raw_tilt_state *state = 0;
double dx, dy, dz;
// Set the LEDs to one of the possible states
if (freenect_sync_set_led(led, 0)) no_kinect_quit();
// Set the tilt angle (in degrees)
if (freenect_sync_set_tilt_degs(tilt, 0)) no_kinect_quit();
// Get the raw accelerometer values and tilt data
if (freenect_sync_get_tilt_state(&state, 0)) no_kinect_quit();
// Get the processed accelerometer values (calibrated to gravity)
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("led[%d] tilt[%d] accel[%lf,%lf,%lf]\n", led, tilt, dx,dy,dz);
sleep(3);
}
}
void CameraNUI::setAngleFromProperty() {
CLOG(LNOTICE) << "Setting angle: " << angle;
freenect_sync_set_tilt_degs(angle, index);
freenect_raw_tilt_state * state;
freenect_sync_get_tilt_state(&state, index);
CLOG(LNOTICE) << "Angle: " << state->tilt_angle << ", state: " << state->tilt_status;
}
/*
** Function called when a key is hit
*/
void KeyboardFunc(unsigned char key, int x, int y) {
//Map keys to move about the environment in the X, Y, Z direction
switch (key) {
case 'a' : moveX+=0.05;break;
case 'd' : moveX-=0.05;break;
case 'w' : moveZ+=0.5;break;
case 's' : moveZ-=0.5;break;
case 'e' : moveY+=0.05;break;
case 'q' : moveY-=0.05;break;
case '8' : if (freenect_sync_set_tilt_degs(tiltAngle+=5, 0)) exit(1);break;
case '2' : if (freenect_sync_set_tilt_degs(tiltAngle-=5, 0)) exit(1);break;
case '5' : if (freenect_sync_set_tilt_degs(tiltAngle=0, 0)) exit(1);break;
case 'h' : moveX=0;moveY=0;moveZ=-5;angleX=0;angleY=0;break;
case 'f' : command = command^1; printf("points deleted: %d\n", cleanPointCloud(rgbToWorld)); break; //freeze frame
case 'x' :
case 'X' :
case 27 :exit(0);
}
}
void *data_in(void *arg) {
int n;
while(data_connected){
char buffer[6];
#ifdef WIN32
n = recv(data_client_socket, (char*)buffer, 1024, 0);
#else
n = read(data_child, buffer, 1024);
#endif
if(n == 6){
if (buffer[0] == 1) { //MOTOR
if (buffer[1] == 1) { //MOVE
int angle;
memcpy(&angle, &buffer[2], sizeof(int));
freenect_sync_set_tilt_degs(ntohl(angle), 0);
}
}
}
}
return NULL;
}
void CameraNUI::setAngle() {
int angle = in_angle.read();
freenect_sync_set_tilt_degs(angle, index);
}
//Connection protocol handler
void *connection_handler(void *arg) {
int value;
int len = 8*10;
char *buff = (char*)malloc(len); //Command buffer
//send_policy_file(data_child);
while(client_connected) {
freenect_network_read(buff, &len);
//If command length is multiple of 6
if(len > 0 && len % 6 == 0){
//Get the number of commands received
int max = len / 6;
int i;
//For each command received
for(i = 0; i < max; i++){
memcpy(&value, &buff[2 + (i*6)], sizeof(int));
value = ntohl(value);
//The BIG switch (Communication protocol)
switch(buff[0 + (i*6)]){
case 0: //CAMERA
switch(buff[1 + (i*6)]){
case 0: //GET DEPTH
sendDepth();
break;
case 1: //GET RAW DEPTH
sendRawDepth();
break;
case 2: //GET RGB
sendVideo();
break;
case 3: //Mirror depth
_depth_mirrored = value;
break;
case 4: //Mirror video
_video_mirrored = value;
break;
case 5: //Min depth
_min_depth = value;
break;
case 6: //Max depth
_max_depth = value;
break;
case 7: //Depth compression
_depth_compression = value;
break;
case 8: //Video compression
_video_compression = value;
break;
}
break;
case 1: //MOTOR
switch(buff[1 + (i*6)]){
case 0: //MOVE
freenect_sync_set_tilt_degs(value, 0);
break;
case 1: //LED COLOR
freenect_sync_set_led((freenect_led_options) value, 0);
break;
case 2: //Accelerometer
sendAccelerometers();
break;
}
break;
}
}
} else { //Command was not multiple of 6 (we received an invalid command)
if(!die) printf("got bad command (%d)\n", len );
client_connected = 0;
}
}
if(!die) {
printf("Disconecting client...\n");
freenect_network_wait();
//waiting for client led status
freenect_sync_set_led((freenect_led_options) 4, 0);
}
return NULL;
}
///functions
int main(int argc, char* argv[])
{
//input parameters
if(argc != 3){
printf("usage: %s <first ip> <second ip>\n", argv[0]);
return EXIT_FAILURE;
}
//set Kinect angles to 0° & set LED colour
if(freenect_sync_set_tilt_degs(0, 0)){
printf("Could not tilt device 0.\n");
return EXIT_FAILURE;
}
if(freenect_sync_set_led(LED_GREEN, 0)){
printf("Could not change LED of device 0.\n");
return EXIT_FAILURE;
}
//set UDP socket
struct sockaddr_in si_other, si_other2;
int s, i, slen=sizeof(si_other);
char buf[BUFLEN];
if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1){
fprintf(stderr, "socket() failed\n");
return 1;
}
//first IP
memset((char *) &si_other, 0, sizeof(si_other));
si_other.sin_family = AF_INET;
si_other.sin_port = htons(PORT);
if (inet_aton(argv[1], &si_other.sin_addr)==0) {
fprintf(stderr, "inet_aton() failed\n");
return 1;
}
//second IP
memset((char *) &si_other2, 0, sizeof(si_other2));
si_other2.sin_family = AF_INET;
si_other2.sin_port = htons(PORT);
if (inet_aton(argv[2], &si_other2.sin_addr)==0) {
fprintf(stderr, "inet_aton() failed\n");
return 1;
}
//set cameras
TDepthCamera mainCam;
createPrimaryCamera(&mainCam, 0);
//get calibration values acquired by calibration program.
FILE* pFile = NULL;
pFile = fopen("calibrationValuesOne.cal", "r");
if(pFile == NULL){
puts("Could not get calibration data.");
}else{
fread(&minZ, sizeof(int), 1, pFile);
fread(&maxZ, sizeof(int), 1, pFile);
}
fclose(pFile);
TVecList mainList;
unsigned int timestamp;
contLoop = 1;
//show current calibration values.
printf("Current calibration values:\nCeiling: %d, Floor: %d\n", maxZ, minZ);
puts("\n\nAre those values correct? [Y/N]");
char tmpChar = getchar();
if(tmpChar == 'N' || tmpChar == 'n'){
contLoop = 0;
puts("\nUse calibration program to correct the values.");
}
fflush(stdin);
//start thread
pthread_t thread;
int rc;
long t = 0;
rc = pthread_create(&thread, NULL, readAsync, (void *)t);
if (rc){
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
//main loop
while(contLoop){
//acquire data for main Kinect & process data
if(updateCamera(&mainCam, ×tamp)){
printf("Could not update feed for device 0.");
return EXIT_FAILURE;
}
if(detectDrone(mainCam.data, &mainList, &vec3DDistance)){
printf("Could not process data for for device 0.");
return EXIT_FAILURE;
}
//display list
system("clear");
puts("Press Enter to exit.\n\n---------------\nLIST:");
displayVecList(&mainList);
//send position to the given IP address
TVec4D* maxVect = maxPointList(&mainList);
if(maxVect != NULL){
writePacket(buf, 'k', maxVect->x, maxVect->y, maxVect->z);
if (sendto(s, buf, BUFLEN, 0, &si_other, slen)==-1){
fprintf(stderr, "sendto() failed\n");
return 1;
}
if (sendto(s, buf, BUFLEN, 0, &si_other2, slen)==-1){
fprintf(stderr, "sendto() failed\n");
return 1;
}
}
}
//close socket
close(s);
//free all data
freeCamera(&mainCam);
//stop kinects
freenect_sync_stop();
//stop pthread
pthread_exit(NULL);
return EXIT_SUCCESS;
}
//Connection protocol handler
void *connection_handler(void *arg) {
int n, value;
//send_policy_file(data_child);
while(client_connected) {
char buff[8*10]; //Command buffer
#ifdef WIN32 //Listen for data (Winsock)
n = recv(data_client_socket, (char*)buff, sizeof(buff), 0);
#else //Listen for data (UNIX)
n = read(data_child, buff, sizeof(buff));
#endif
//If command length is multiple of 6
if(n > 0 && n % 6 == 0){
//Get the number of commands received
int max = n / 6;
int i;
//For each command received
for(i = 0; i < max; i++){
memcpy(&value, &buff[2 + (i*6)], sizeof(int));
value = ntohl(value);
//The BIG switch (Communication protocol)
switch(buff[0 + (i*6)]){
case 0: //CAMERA
switch(buff[1 + (i*6)]){
case 0: //GET DEPTH
sendDepth();
break;
case 1: //GET RGB
sendVideo();
break;
case 2: //Mirror depth
_depth_mirrored = value;
break;
case 3: //Mirror video
_video_mirrored = value;
break;
case 4: //Min depth
_min_depth = value;
break;
case 5: //Max depth
_max_depth = value;
break;
}
break;
case 1: //MOTOR
switch(buff[1 + (i*6)]){
case 0: //MOVE
freenect_sync_set_tilt_degs(value, 0);
break;
case 1: //LED COLOR
freenect_sync_set_led((freenect_led_options) value, 0);
break;
case 2: //Accelerometer
sendAccelerometers();
break;
}
break;
}
}
} else { //Command was not multiple of 6 (we received an invalid command)
printf("got bad command (%d)\n", n);
client_connected = 0;
}
}
printf("Disconecting client...\n");
//waiting for client led status
freenect_sync_set_led((freenect_led_options) 4, 0);
return NULL;
}
