tilt.c
Go to the documentation of this file.00001 /* 00002 * This file is part of the OpenKinect Project. http://www.openkinect.org 00003 * 00004 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file 00005 * for details. 00006 * 00007 * This code is licensed to you under the terms of the Apache License, version 00008 * 2.0, or, at your option, the terms of the GNU General Public License, 00009 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, 00010 * or the following URLs: 00011 * http://www.apache.org/licenses/LICENSE-2.0 00012 * http://www.gnu.org/licenses/gpl-2.0.txt 00013 * 00014 * If you redistribute this file in source form, modified or unmodified, you 00015 * may: 00016 * 1) Leave this header intact and distribute it under the same terms, 00017 * accompanying it with the APACHE20 and GPL20 files, or 00018 * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or 00019 * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file 00020 * In all cases you must keep the copyright notice intact and include a copy 00021 * of the CONTRIB file. 00022 * 00023 * Binary distributions must follow the binary distribution requirements of 00024 * either License. 00025 */ 00026 00027 #include <stdio.h> 00028 #include <stdlib.h> 00029 #include <string.h> 00030 #include <unistd.h> 00031 #include <math.h> 00032 00033 #include "freenect_internal.h" 00034 00035 // The kinect can tilt from +31 to -31 degrees in what looks like 1 degree increments 00036 // The control input looks like 2*desired_degrees 00037 #define MAX_TILT_ANGLE 31 00038 #define MIN_TILT_ANGLE (-31) 00039 00040 #define GRAVITY 9.80665 00041 00042 int freenect_set_tilt_degs(freenect_device *dev, double angle) 00043 { 00044 int ret; 00045 uint8_t empty[0x1]; 00046 00047 angle = (angle<MIN_TILT_ANGLE) ? MIN_TILT_ANGLE : ((angle>MAX_TILT_ANGLE) ? MAX_TILT_ANGLE : angle); 00048 angle = angle * 2; 00049 00050 ret = fnusb_control(&dev->usb_motor, 0x40, 0x31, (uint16_t)angle, 0x0, empty, 0x0); 00051 return ret; 00052 } 00053 00054 int freenect_set_led(freenect_device *dev, freenect_led_options option) 00055 { 00056 int ret; 00057 uint8_t empty[0x1]; 00058 ret = fnusb_control(&dev->usb_motor, 0x40, 0x06, (uint16_t)option, 0x0, empty, 0x0); 00059 return ret; 00060 } 00061 00062 double freenect_get_tilt_degs(freenect_raw_device_state *state) 00063 { 00064 return ((double)state->tilt_angle) / 2.; 00065 }