Servo weird behaviour - Broadcom and Linux 2.4

Supporting 2.6 and up
Mike_S

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby Mike_S » Fri Jan 01, 2010 3:24 pm

Hi Jeff,

The code I attached in one of my previous posts worked fine on my asus.
I wonder if it's possible that you have a phidget board that has different product ID (#define USB_PRODUCT 0x38 ).

Anyway, it would be worth to add some debug code to the loop so at least you know what usb devices are connected and if your system has detected any Phidget vendor devices

Code: Select all

      for (bus = usb_busses; bus; bus = bus->next) {
                printf("Scanning %s\n", bus->dirname);
                for (dev = bus->devices; dev; dev = dev->next) {
                        printf("\tdevice: %s\n", dev->filename);
                        if (dev->descriptor.idVendor == USB_VENDOR)
                                printf("\t OK, vendor match\n");
                        if (dev->descriptor.idVendor != USB_VENDOR ||
                                dev->descriptor.idProduct != USB_PRODUCT)
                                        continue;
                        goto FOUND;
                }
        }



I haven't compiled it but it should work fine, I've just added couple of printfs.

If you need more help then I can connect my servos, see what's in dmesg etc.

Mike

jeffrafter

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby jeffrafter » Fri Jan 01, 2010 11:28 pm

Hi Mike!

Thanks for the response, I got it to work! It was the product ID (it needed to be 0x39). For anyone else checking this here are the steps:

1) unplug the usb cable
2) reboot your router
3) dmesg -c
4) plug in the usb servo
5) dmesg

This should give you the info you need. If nothing shows up then the Servo is not being picked up and you have a different problem. I toyed around some and made a quick video... your code is awesome :)

http://www.vimeo.com/8460198

Thanks again..
Jeff

Mike_S

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby Mike_S » Sat Jan 02, 2010 6:20 am

Hi Jeff,

You are welcome, I am glad it worked. What project are you working on?
Maybe I should modify the code so it displays all Phidget products found in the system, it seems that hard coding product_id is not a good idea after all.
Asus router is great for experimenting with Phidgets.
Recently I bought Sheeva plug and this is another useful small device that can be your home server and Phidget controlling station as well. It is much more powerful that Asus (512MB ram, 1.2 GHz CPU), great fun.
I haven't connected Phidgets to it yet, but I'm sure it would be even easier that Asus.

Thanks,

Mike

jeffrafter

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby jeffrafter » Mon Apr 11, 2011 4:05 am

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: Select all

#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;
}



User avatar
Patrick
Lead Developer
Posts: 3165
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby Patrick » Mon Apr 11, 2011 10:27 am

packet_type is wrong - it should be 0x00 for a normal packet and 0x10 for a min/max packet, but you are setting it equal to index.

-Patrick

jeffrafter

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby jeffrafter » Mon Apr 11, 2011 10:44 am

Thanks Patrick!

Still not quite working. I wasn't sure if I needed to send the minmax init packet I am sending on the adv servo controller. Can I skip that? Is there something I am missing along the lines of Engaging the motor at the index? I was trying to get through the MACROs but my C is rusty :)

Thanks so much, I am showing this to some kids at an elementary school today!

Jeff

User avatar
Patrick
Lead Developer
Posts: 3165
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby Patrick » Mon Apr 11, 2011 11:57 am

don't send the min/max packet.

buffer[0] should be 0x00 for servo 0.
buffer[1] (flags) should be 0x00 to enable the servo.

Just make sure your accel/velocity/pwm are within valid ranges.

ie accel is 0x0001 - 0x4000 and velocity is 0x0000 - 0x4000.
pwm is 0x0000 - 0x8000.

just set them somewhere central and you should see the servo move.

-Patrick

jeffrafter

Re: Servo weird behaviour - Broadcom and Linux 2.4

Postby jeffrafter » Mon Apr 11, 2011 1:23 pm

Hey Patrick! It all works. I had a number of problems but the biggest was my accel which was 0. Here is the final code for all of you googlers. It can still be cleaned up quite a bit I think :)

Thanks so much!

Code: Select all

#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
#define SERVO_VERSION      6

static int pdd_iid = 0;

static int setpos(struct usb_dev_handle *udev, int index, double pos)
{
// if PHIDID_ADVANCEDSERVO_8MOTOR 0x3A

        fprintf(stdout, "Initializing advanced servo controller\n");

        char buf[8];
        int ret = 0;
        int pwm = 0, velocity = 0, accel = 0;
        bzero(buf, sizeof buf);

        buf[0] = (index << 5);
        buf[1] = 0;

        pwm = (int)((pos * 10.6 + 243.8) * 12);
        int velocityMaxLimit = (50/12.0) * 16384; //68266.67 us/s
        int velocityMin = 0;
        velocity = (int)(((128/12.0*316) / velocityMaxLimit) * 16384);
        accel = (int)0x200;

        fprintf(stdout, "Preparing the position command\n\nindex: %d\nvelocity: %d\naccel: %d\npwm: %d\n", index, velocity, accel, pwm);

        buf[2] = ((pwm >> 8) & 0xff);
        buf[3] = (pwm & 0xff);
        buf[4] = ((velocity >> 8) & 0xff);
        buf[5] = (velocity & 0xff);
        buf[6] = ((accel >> 8) & 0xff);
        buf[7] = (accel & 0xff);

        fprintf(stdout, "Sending the position command\n");

        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);

        fprintf(stdout, "Sent the position command: %d\n", ret);

        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;
        int index;

        if (argc < 2) {
                fprintf(stderr, "Phidget servo controller version %d\n\nNeed arg: index (int), new_pos (float)\n",
                        SERVO_VERSION);
                exit(1);
        }
        index = atoi(argv[1]);
        if (index < 0 || index > 7) {
                fprintf(stderr, "Invalid range, servo min index: %d, max index: %d\n",
                        0, 7);
                exit(1);
        }
        pos = atol(argv[2]);
        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);
        }
        int ret = 0;
        if((ret = usb_claim_interface(udev, pdd_iid)) < 0)
        {
          fprintf(stderr, "usb_claim_interface failed with error code: %d \"%s\"", ret, strerror(-ret));
          if((ret = usb_close(udev)) < 0)
          {
             fprintf(stderr, "usb_close failed with error code: %d \"%s\"", ret, strerror(-ret));
          }
          exit(1);
        }

        ret = setpos(udev, index, pos);
        fprintf(stdout, "Setting position to %i.\n\n Return: %i\n", pos, ret);
        return 0;
}


Return to “Linux”

Who is online

Users browsing this forum: No registered users and 1 guest