static void wait_for_usb(void)
{
struct timeval to, now;
usb_dev_handle *dev;
gettimeofday(&to, NULL);
to.tv_sec += timeout;
while (1) {
usb_rescan();
dev = open_usb(0, 0);
if (dev) {
if (!need_removal)
return;
usb_close(dev);
} else {
need_removal = 0;
}
if (timeout) {
gettimeofday(&now, NULL);
if (now.tv_sec > to.tv_sec)
break;
if (now.tv_sec == to.tv_sec &&
now.tv_usec > to.tv_usec)
break;
}
if (usleep(interval_us) < 0) {
perror("usleep");
exit(1);
}
}
fprintf(stderr, "timeout\n");
exit(1);
}
void serial_error() {
serial_errs += 1;
if (serial_errs == MAX_ERRORS) {
ROS_ERROR("Several errors from roboclaw, restarting");
roboclaw_restart_usb();
open_usb();
serial_errs = 0;
}
}
int main(int argc, char *argv[]) {
Display *d = XOpenDisplay(NULL);
if (!d) {
printf("Unable to open display\n");
return;
}
#ifdef USB_PIXEL
usb_dev_handle *handle = NULL;
int usb_present = open_usb(&handle);
if (!handle) {
printf("Unable to open usb device, proceeding anyway...\n");
}
#endif
int radius = RADIUS;
init_shm(d, radius);
init_xinput(d);
int x = -1;
int y = -1;
int old_x = -1;
int old_y = -1;
struct rgb_color color;
color.alpha = 255;
uint8_t buf[3];
while(1) {
wait_for_movement(d, &x, &y);
if (x != old_x || y != old_y) {
refresh_image(d, x, y, radius);
get_pixel_color(d, x, y, &color, radius);
printf("%d/%d\t(%d/%d/%d)\n", x, y, color.red, color.green, color.blue);
#ifdef USB_PIXEL
if (usb_present) {
buf[0] = color.red;
buf[1] = color.green;
buf[2] = color.blue;
int8_t sent = usb_control_msg(handle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_OUT, CUSTOM_RQ_SET_RGB, 0, 0, buf, 3, 100);
if (sent < 0) {
printf("Lost contact to USB device\n");
usb_present = 0;
handle = NULL;
}
}
#endif
old_x = x;
old_y = y;
}
}
XCloseDisplay(d);
}
int main(int argc, const char **argv)
{
usb_dev_handle *dev;
uint16_t build;
char buf[BUF_SIZE+1]; /* +1 for terminating \0 */
int len;
if (argc != 1)
usage(*argv);
dev = open_usb();
if (!dev) {
fprintf(stderr, ":-(\n");
return 1;
}
if (idbg_print_id(stdout, dev) < 0)
return 1;
printf("\n");
len = idbg_get_build_number(dev, &build);
if (len < 0)
return 1;
switch (len) {
case 0:
build = 0; /* we'll probably fail idbg_get_build_date */
break;
case 2:
break;
default:
fprintf(stderr, "build number length (%d) != 2\n", len);
return 1;
}
len = idbg_get_build_date(dev, buf, BUF_SIZE);
if (len < 0)
return 1;
if (len) {
buf[len] = 0;
printf("%10s%s #%u\n", "", buf, (unsigned) build);
}
return 0;
}
bool PS3EYECam::init(uint32_t width, uint32_t height, uint8_t desiredFrameRate)
{
uint16_t sensor_id;
// open usb device so we can setup and go
if(handle_ == NULL)
{
if( !open_usb() )
{
return false;
}
}
//
if(usb_buf == NULL)
usb_buf = (uint8_t*)malloc(64);
// find best cam mode
if((width == 0 && height == 0) || width > 320 || height > 240)
{
frame_width = 640;
frame_height = 480;
} else {
frame_width = 320;
frame_height = 240;
}
frame_rate = ov534_set_frame_rate(desiredFrameRate, true);
frame_stride = frame_width * 2;
//
/* reset bridge */
ov534_reg_write(0xe7, 0x3a);
ov534_reg_write(0xe0, 0x08);
#ifdef _MSC_VER
Sleep(100);
#else
nanosleep((const struct timespec[]){{0, 100000000}}, NULL);
int main(int argc, char *argv[]) {
usb_dev_handle *handle = NULL;
int usb_present = open_usb(&handle);
if (!handle) {
perror("Unable to open usb device");
return 1;
}
argc--;
argv++;
while (argc) {
int result = process_command(handle, argv[0]);
if (result == 1) {
usb_close(handle);
return 1;
} else if (result == 2) {
fprintf(stderr, "Unable to parse command line item: %s\n", argv[0]);
}
argc--;
argv++;
}
usb_close(handle);
return 0;
}
RoboclawNode() : nh(), priv_nh("~") {
boost::mutex::scoped_lock lock(claw_mutex_);
priv_nh.param<std::string>("port", port, "/dev/roboclaw");
priv_nh.param<std::string>("base_frame_id", base_frame_id, "base_footprint");
if (!priv_nh.getParam("baud_rate", baud_rate)) {
baud_rate = 1000000;
}
if (!priv_nh.getParam("rate", update_rate)) {
update_rate = 20;
}
if (!priv_nh.getParam("base_width", base_width)) {
base_width = 0.265;
}
if (!priv_nh.getParam("ticks_per_metre", ticks_per_m)) {
ticks_per_m = 21738;
}
if (!priv_nh.getParam("robot_direction", robot_dir)) {
robot_dir = 0;
}
if (!priv_nh.getParam("max_acceleration", max_accel)) {
max_accel = 1.0;
}
if (!priv_nh.getParam("KP", KP)) {
KP = 0.1;
}
if (!priv_nh.getParam("KI", KI)) {
KI = 0.5;
}
if (!priv_nh.getParam("KD", KD)) {
KD = 0.25;
}
if (!priv_nh.getParam("QPPS", QPPS)) {
QPPS = 11600;
}
if (!priv_nh.getParam("left_motor_direction", left_dir)) {
left_dir = 1;
}
if (!priv_nh.getParam("right_motor_direction", right_dir)) {
right_dir = 1;
}
if (!priv_nh.getParam("last_odom_x", x)) {
x = 0.0;
}
if (!priv_nh.getParam("last_odom_y", y)) {
y = 0.0;
}
if (!priv_nh.getParam("last_odom_theta", theta)) {
theta = 0.0;
}
ROS_INFO_STREAM("Starting roboclaw node with params:");
ROS_INFO_STREAM("Port:\t" << port);
ROS_INFO_STREAM("Baud rate:\t" << baud_rate);
ROS_INFO_STREAM("Base Width:\t" << base_width);
ROS_INFO_STREAM("Ticks Per Metre:\t" << ticks_per_m);
ROS_INFO_STREAM("KP:\t" << KP);
ROS_INFO_STREAM("KI:\t" << KI);
ROS_INFO_STREAM("KD:\t" << KD);
ROS_INFO_STREAM("QPPS:\t" << QPPS);
ROS_INFO_STREAM("Robot Dir:\t" << robot_dir);
ROS_INFO_STREAM("X:\t" << x);
ROS_INFO_STREAM("Y:\t" << y);
ROS_INFO_STREAM("Theta:\t" << theta);
serial_errs = 0;
ser.reset(new USBSerial());
open_usb();
claw.reset(new RoboClaw(ser.get()));
max_accel_qpps = (int) (max_accel * ticks_per_m);
last_motor = ros::Time::now();
target_left_qpps = target_right_qpps = 0;
vx = vth = 0;
last_odom = ros::Time::now();
last_enc_left = last_enc_right = 0;
quaternion.x = 0.0;
quaternion.y = 0.0;
odom.header.frame_id = "odom";
odom.child_frame_id = base_frame_id;
odom.pose.pose.position.z = 0.0;
cmd_vel_sub = nh.subscribe("cmd_vel", 1, &RoboclawNode::twistCb, this);
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
joint_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
state_pub = nh.advertise<roboclaw_driver::RoboClawState>("roboclaw_state", 1);
try {
claw->SetM1Constants(address, KD, KP, KI, QPPS);
claw->SetM2Constants(address, KD, KP, KI, QPPS);
claw->ReadVersion(address, &roboclaw_version);
ROS_INFO_STREAM("Connected to: " << roboclaw_version);
claw->ResetEncoders(address);
} catch (USBSerial::Exception &e) {
ROS_WARN("Problem setting PID constants (error=%s)", e.what());
serial_error();
}
js.name.push_back("base_l_wheel_joint");
js.name.push_back("base_r_wheel_joint");
js.position.push_back(0.0);
js.position.push_back(0.0);
js.effort.push_back(0.0);
js.effort.push_back(0.0);
js.header.frame_id = "base_link";
calib_server = nh.advertiseService("motor_calibrate", &RoboclawNode::calib_callback, this);
}
int main (int argc, char * argv[]) {
int fd;
struct termios tio;
int i;
_psu_type = TYPE_AX760;
printf("Corsair AXi Series PSU Monitor\n");
printf("(c) 2014 Andras Kovacs - [email protected]\n");
printf("-------------------------------------------\n\n");
if (argc < 2) {
printf("usage: %s <serial port device>\n", argv[0]);
return 0;
}
fd = open_usb(argv[1]);
if(fd == -1)
{
return -1;
}
memset(&tio, 0, sizeof(tio));
tio.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
tio.c_iflag = IGNPAR;
tio.c_oflag = 0;
tio.c_lflag = 0;
tio.c_cc[VTIME]= 0;
tio.c_cc[VMIN] = 1;
tcsetattr(fd, TCSANOW, &tio);
tcflush(fd, TCIOFLUSH);
float f;
if (setup_dongle(fd) == -1) exit(-1);
if (read_psu_fan_speed(fd, &f) == -1) exit(-1);
printf("Fan speed: %0.2f RPM\n", f);
if (read_psu_temp(fd, &f) == -1) exit(-1);
printf("Temperature: %0.2f °C\n", f);
if (read_psu_main_power(fd) == -1) exit(-1);
printf("Voltage: %0.2f V\n", _psumain.voltage);
printf("Current: %0.2f A\n", _psumain.current);
printf("Input power: %0.2f W\n", _psumain.inputpower);
printf("Output power: %0.2f W\n", _psumain.outputpower);
if (_psu_type == TYPE_AX1500)
printf("Cable type: %s\n", (_psumain.cabletype ? "20 A" : "15 A"));
printf("Efficiency: %0.2f %%\n", _psumain.efficiency);
if (read_psu_rail12v(fd) == -1) exit(-1);
int chnnum = (_psu_type == TYPE_AX1500 ? 10 : ((_psu_type == TYPE_AX1200) ? 8 : 6));
for (i = 0; i < chnnum; i++) {
printf("PCIe %02d Rail: %0.2f V, %0.2f A, %0.2f W, OCP %s (Limit: %0.2f A)\n", i, _rail12v.pcie[i].voltage,
_rail12v.pcie[i].current, _rail12v.pcie[i].power, (_rail12v.pcie[i].ocp_enabled ? "enabled " : "disabled"), _rail12v.pcie[i].ocp_limit);
}
printf("ATX Rail: %0.2f V, %0.2f A, %0.2f W, OCP %s (Limit: %0.2f A)\n", _rail12v.atx.voltage,
_rail12v.atx.current, _rail12v.atx.power, (_rail12v.atx.ocp_enabled ? "enabled " : "disabled"), _rail12v.atx.ocp_limit);
printf("Peripheral Rail: %0.2f V, %0.2f A, %0.2f W, OCP %s (Limit: %0.2f A)\n", _rail12v.peripheral.voltage,
_rail12v.peripheral.current, _rail12v.peripheral.power, (_rail12v.peripheral.ocp_enabled ? "enabled " : "disabled"), _rail12v.peripheral.ocp_limit);
if(read_psu_railmisc(fd) == -1) exit(-1);
printf("5V Rail: %0.2f V, %0.2f A, %0.2f W\n", _railmisc.rail_5v.voltage, _railmisc.rail_5v.current, _railmisc.rail_5v.power);
printf("3.3V Rail: %0.2f V, %0.2f A, %0.2f W\n", _railmisc.rail_3_3v.voltage, _railmisc.rail_3_3v.current, _railmisc.rail_3_3v.power);
close(fd);
return 0;
}
int main (int argc, char * argv [])
{
int x;
uchar *buf = (uchar*) malloc (65536);
char *pc;
assert (buf != 0);
rc = libusb_init (NULL);
assert (rc == 0);
if (NULL == (realpath (argv [0], szBasePath)))
{
printf ("Can't get realpath\n");
return (1);
}
for (pc = strchr (szBasePath, '\0'); *pc != '/'; pc--) *pc = '\0';
FN = &FN_sun7i; // A20 by default.
while (argc > 1)
{
if (strcmp (argv [1], (char *) "-t") == 0)
{
USBTests (buf);
goto bye;
}
if (strcmp (argv [1], (char *) "-H") == 0)
{
H3_Tests (buf);
goto bye;
}
if (strcmp (argv [1], (char *) "-l") == 0)
{
Lime_Tests (buf);
goto bye;
}
if (strcmp (argv [1], (char *) "-a") == 0)
{
A20_Tests (buf);
goto bye;
}
if (strcmp (argv [1], (char *) "-h") == 0)
{
printf ("no flags = update boot0 and boot1.\n");
printf ("-x = allow force continue when error.\n");
printf ("-e = erase chip before boot update.\n");
printf ("-r = read all NAND, save to NAND.DAT.\n");
printf ("-w = write NAND.DAT to device NAND.\n");
printf ("-t = USB tests.\n");
printf ("-i <file list> = create NAND MBR, write MBR and files to NAND,\n");
printf (" each file is a partition to write, the file name is used as\n");
printf (" the partition name in the MBR.\n");
printf (" Each file entry consists of a name and start and size enclosed in quotes.\n");
printf (" The start is the sector key (0 = use next available sector).\n");
printf (" The size is the partition size in sectors (0 = use file size).\n");
printf (" e.g. bootfix -i \"/data/boot 2048 0\" \"/data/rootfs 0 0\" \"/data/extra 0 0\"\n");
goto bye;
}
if (strcmp (argv [1], (char *) "-x") == 0)
forceable = 1;
else if (strcmp (argv [1], (char *) "-e") == 0)
bEraseReqd = 1;
else if (strcmp (argv [1], (char *) "-r") == 0)
{
readNAND = 1;
if (argc > 2)
strcpy (NAND_FID, argv [2]);
break;
}
else if (strcmp (argv [1], (char *) "-w") == 0)
{
writeNAND = 1;
if (argc > 2)
strcpy (NAND_FID, argv [2]);
break;
}
else if (strcmp (argv [1], (char *) "-i") == 0)
{
loadNAND = 1;
part_cnt = argc - 2;
if (part_cnt > 16)
part_cnt = 16;
BOJLoadNANDCheck (part_cnt, &argv [2], part_start, part_secs);
bEraseReqd = true;
break;
}
argc--;
argv++;
}
handle = open_usb (); // stage 1
stage_1 (handle, buf);
handle = close_usb (handle);
printf ("Re-opening"); // stage 2
fflush (stdout);
for (x = 0; !handle; x++)
{
usleep (1000000); // wait 1 sec
printf (".");
fflush (stdout);
handle = open_usb (x < 10); // try to open
}
printf ("done\n");
stage_2 (handle, buf);
handle = close_usb (handle);
printf ("All done\n");
bye:
libusb_exit (NULL);
return 0;
}
int main(int argc, char **argv)/*the main function*/
{
/** Initialize ROS node.
* It must be declared in the begining of the program because of TF Listener
*This specifies the name of the node **/
ros::init(argc, argv, "telosb_node");
//create a node handle
ros::NodeHandle nh ("~");
std::string port;//"/dev/telosb";
nh.getParam("port",port);
printf ("Starting TelosB Node ... \n");
//char* device = "/dev/ttyUSB0";
//char* device = "/dev/telosb";
char *device = (char *) malloc(100);;
printf ("set device name ... \n");
strcpy(device,port.c_str());
//printf ("cout ... \n");
std::cout <<device<<std::endl;
if (device == NULL )
device = "/dev/ttyUSB0";
printf("attempting to open %s", device);
int baudrate = 115200;
printf ("open USB port ... \n");
int b = open_usb(device, baudrate);
//first, discard any previous data
flush_usb();
if (b==1)
printf ("USB Port Open \n");
if (b==-1)
printf ("USB Port NOT Open \n");
int r, i, j;
BYTE buffer[BUFSIZE];
BYTE read_buffer[BUFSIZE];
int received_bytes;
int count =0;
//must be defined after opening the USB port!!
ros::Publisher telosb_publisher= nh.advertise<telosb::TelosBMsg>("telosb_topic",1000);
while (ros::ok())
{
//fflush(stdout);
//printf ("before read_usd \n");
read_usb(read_buffer, BUFSIZE, &r);
//printf ("after read_usd \n");
//printf("r=%d\n ",r);
printf("count=%d\n ",count);
//printf("%s\n",buffer);
//printf("current index = %d \n ", current_index);
if(r!=0){
//fflush(stdout);
int offset=count;
count = count+r;
int i=0;
//printf ("before for ( i = 0; i<r; i++) %d, offset = %d\n", r, offset);
for ( i = 0; i<r; i++){
buffer[i+offset]=read_buffer[i];
}
//printf ("after for ( i = 0; i<r; i++) \n");
//printf ("before if (count==12) \n");
if (count==12){
count = 0;
received_bytes=0;//re-init received_bytes
//fflush(stdout);
printf ("before id = \n");
int id = (buffer[2]-'0')+((buffer[1]-'0')*10)+(buffer[0]-'0')*100;
int light = (buffer[5]-'0')+((buffer[4]-'0')*10)+(buffer[3]-'0')*100;
int temp = (buffer[8]-'0')+((buffer[7]-'0')*10)+(buffer[6]-'0')*100;
int humidity = (buffer[11]-'0')+((buffer[10]-'0')*10)+(buffer[9]-'0')*100;
//printf ("r=%d \n", r);
//printf ("Sensor reading \n");
printf ("before Sensor ID = \n");
printf ("Sensor ID : %d \n", id);
printf ("Light : %d Lux\n", light);
printf ("Temperature: %d °C\n", temp);
printf ("Humidity : %d %%\n\n", humidity);
//printf("%d\n ",i);
telosb::TelosBMsg telosb_msg;
telosb_msg.sensorID=id;
telosb_msg.temperature=temp;
telosb_msg.humidity=humidity;
telosb_msg.light=light;
ROS_INFO("\n[ID = %d]: temperature: %d C, light: %d Lux, Humidity: %d \n",
telosb_msg.sensorID, telosb_msg.temperature, telosb_msg.light, telosb_msg.humidity);
telosb_publisher.publish(telosb_msg);
ros::spinOnce();
//loop_rate.sleep();
}
}
}
printf (" read usb completed. \n");
return 1;
}
