This is now a really old post but I keep on having more fun with OpenWRT and now a Advanced Servo Controller (8x). But my code just won't cause anything to happen on the servos. No error message is returned, all of the calls end up with positive results. This code has a lot of assumptions in it, including that I am only trying to move index 0.
Code:
#include <stdio.h>
#include <strings.h>
#include "usb.h"
#define USB_VENDOR 0x6C2
#define USB_PRODUCT_SINGLE 0x39
#define USB_PRODUCT 0x3A
#define SERVO_MINPOS -23
#define SERVO_MAXPOS 232
static int pdd_iid = 0;
static int setpos(struct usb_dev_handle *udev, double pos)
{
// if PHIDID_SERVO_1MOTOR 0x39
/*
char buf[8];
int ret, pulse = (int)(pos * 10.6 + 243.8);
bzero(buf, sizeof buf);
buf[0] = pulse & 0xFF;
buf[1] = pulse >> 8;
ret = usb_control_msg(udev, USB_ENDPOINT_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
USB_REQ_SET_CONFIGURATION, 0x0200, pdd_iid,
(char *) buf, 6, 500);
*/
// if PHIDID_SERVO_4MOTOR 0x38
/*
char buf[8];
int ret, pulse0 = (int)(pos * 10.6 + 243.8);
int pulse1 = 0;
int pulse2 = 0;
int pulse3 = 0;
bzero(buf, sizeof buf);
buf[0] = (unsigned char)(pulse0 & 0xFF);
buf[1] = (unsigned char)((pulse0 >> 8) & 0x0F);
buf[2] = (unsigned char)(pulse1 & 0xFF);
buf[1] |= (unsigned char)((pulse1 >> 4) & 0xF0);
buf[3] = (unsigned char)(pulse2 & 0xFF);
buf[4] = (unsigned char)((pulse2 >> 8) & 0x0F);
buf[5] = (unsigned char)(pulse3 & 0xFF);
buf[4] |= (unsigned char)((pulse3 >> 4) & 0xF0);
ret = usb_control_msg(udev, USB_ENDPOINT_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
USB_REQ_SET_CONFIGURATION, 0x0200, pdd_iid,
(char *) buf, 6, 500);
*/
// if PHIDID_ADVANCEDSERVO_8MOTOR 0x3A
char buf[8];
int ret = 0;
int index = 0;
int pwm = 0, velocity = 0, accel = 0, flags = 0;
int packet_type = index;
int packet_counter = 1;
packet_counter &= 0x0F;
// NO_RAMPING_FLAG_ADVSERVO 0x80
// MOTOR_DISABLED_ADVSERVO 0x40
// flags |= 0x80;
// flags |= 0x40;
bzero(buf, sizeof buf);
//2-bit index, 2-bit packet type, 4-bit counter (which is always 0)
buf[0] = (index << 5) | packet_type | packet_counter;
buf[1] = flags;
pwm = (int)((pos * 10.6 + 243.8) * 12);
int maxpwm = 243 * 128/12.0;
int minpwm = 23 * 128/12.0;
int velocityMaxLimit = (50/12.0) * 16384; //68266.67 us/s
int velocityMin = 0;
int accelerationMax = (50/12.0) / 0.02 * 16384; //3413333.33 us/s^2
int accelerationMin = (50/12.0) / 0.02; //min velocity over time integration increment - 208.33 us/s^2
buf[2] = (unsigned char)((minpwm >> 8) & 0xff);
buf[3] = (unsigned char)(minpwm & 0xff);
buf[4] = (unsigned char)((maxpwm >> 8) & 0xff);
buf[5] = (unsigned char)(maxpwm & 0xff);
buf[6] = 0;
buf[7] = 0;
// Init?
ret = usb_control_msg(udev, USB_ENDPOINT_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
USB_REQ_SET_CONFIGURATION, 0x0200, pdd_iid,
(char *) buf, 8, 500);
if(ret < 0)
{
switch(ret)
{
case -ETIMEDOUT: //important case?
fprintf(stderr, "Timeout");
break;
case -ENODEV:
//device is gone - unplugged.
fprintf(stderr, "Device was unplugged - detach.");
break;
default:
fprintf("Sending usb_control_msg failed with error code: %d \"%s\"", ret, strerror(-ret));
break;
}
}
velocity = (int)(((128/12.0*316) / velocityMaxLimit) * 16384);
accel = (int)(((accelerationMax / 2) / accelerationMax) * 16384);
buf[2] = (unsigned char)((pwm >> 8) & 0xff);
buf[3] = (unsigned char)(pwm & 0xff);
buf[4] = (unsigned char)((velocity >> 8) & 0xff);
buf[5] = (unsigned char)(velocity & 0xff);
buf[6] = (unsigned char)((accel >> 8) & 0xff);
buf[7] = (unsigned char)(accel & 0xff);
ret = usb_control_msg(udev, USB_ENDPOINT_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
USB_REQ_SET_CONFIGURATION, 0x0200, pdd_iid,
(char *) buf, 8, 500);
if(ret < 0)
{
switch(ret)
{
case -ETIMEDOUT: //important case?
fprintf(stderr, "Timeout");
break;
case -ENODEV:
//device is gone - unplugged.
fprintf(stderr, "Device was unplugged - detach.");
break;
default:
fprintf("Sending usb_control_msg failed with error code: %d \"%s\"", ret, strerror(-ret));
break;
}
}
return ret;
}
struct usb_bus *bus;
struct usb_device *dev;
usb_dev_handle *udev;
int main(int argc, char **argv)
{
double pos;
if (argc < 2) {
fprintf(stderr, "Phidget servo controller version %d\n\nNeed arg: new_pos (float)\n",
SERVO_VERSION);
exit(1);
}
pos = atol(argv[1]);
if (pos < SERVO_MINPOS || pos > SERVO_MAXPOS) {
fprintf(stderr, "Invalid range, servo min pos: %d, max pos: %d\n",
SERVO_MINPOS, SERVO_MAXPOS);
exit(1);
}
usb_init();
usb_find_busses();
usb_find_devices();
for (bus = usb_busses; bus; bus = bus->next) {
for (dev = bus->devices; dev; dev = dev->next) {
if (dev->descriptor.idVendor != USB_VENDOR ||
dev->descriptor.idProduct != USB_PRODUCT)
continue;
goto FOUND;
}
}
fprintf(stderr, "Phidget device has not been found.\n");
exit(1);
FOUND:
udev = usb_open(dev);
if (!udev) {
fprintf(stderr, "Cannot open usb device.\n");
exit(1);
}
fprintf(stdout, "Setting position to %d.\n\n Return: %d", pos, setpos(udev, pos));
return 0;
}
Login
Register
FAQ
Members
