Wednesday, March 25, 2015

Servo Abstraction for Raspberry Pi Java PWM Servo Driver


After getting the servo output in the last post (Raspberry Pi Java PCA9685 PWM Driver - First servo pulses output) and validating the output was reasonably accurate I had to look at the way I wanted to update the servo values. In the initial example in the previous post I was using a hard coded output.

My goal is some other part of the application will set a specific servos angle without caring about constraints such as range or calibration. To achieve this I added a new interface called Servo to provide access to information about specific servo instances. Originally I did not have this. It's absent in earlier UML in my blog. However I decided the servo was the best place to encapsulate servo calibration data. I want each part of the application to have the appropriate separation of concern. An overview of the abstraction I have currently is as follows.

  • Servo
    • Provides abstraction of individual servo data such as calibration data
    • Provides an interface to set the desired angle on  the servo
    • Methods to provide calibrated pulse length
  • Servo Driver
    • The servo driver provides methods to set the servo update frequency that al servos require
    • Methods to update the position of one or multiple servos
  • Comm Device
    • The device provides an interface for the actual communication device. 
    • The device handles the protocol for communication to the device (i2C in this case)
    • Actual Device is PCA9685 based device Adafruit PWM servo driver
  • Physical Servo
    • The actual servo

My goal is some other part of the application will set a specific servos angle without caring about constraints such as range or calibration. To achieve this I added a new interface called Servo to provide access to information about specific servo instances. Originally I did not have this. It's absent in earlier UML in my blog. However I decided the servo was the best place to encapsulate servo calibration data.

In this post I'll share what I have so far for the servo and servo driver.

Servo

Just as an example, different servos have different ranges. The typical RC servo pulse is 1ms to 2ms where 1ms is full left and 2ms is full right.  If a servo is 180 of range -90degs from center to 90 degs from center. A range of 120 is -60degs to 60degs.

I want to be able to interact with servos using degs of angle. This will be more practical for creating leg movement sequencing. I want to be able to set -60 and know the servo regardless of range will move to that angle. It actually would be 1ms pulse on a 120 deg servo but 1.33ms on a 180 deg servo.

With that in mind it made sense to encapsulate all the servo calibration data in the servo object. The calibration fields are
  1. range - the maximum range of the servo in total degrees.
  2. center - where the center of the servo is as installed. This depends on the installation and how the servo control arm/wheel is fitted on the servo. This calibration allows the tuning of the center position of the servo
  3. lowLimit - Limits the travel of the servo. In the robot the servo is constrained by the physical leg movement before it reaches its range of travel. The limit prevents damage to the servo.
  4. highLimit - same as above but for high

Therefore the servo should provide the pulse length pre calibrated with the calibration data above. Most of the methods are accessors to the calibration data fields. However the getPulseLength method is more interesting

    public int getPulseLength(int argAngle) {
        // limit angle
        if(argAngle > getHighLimit()){
            int newAngle = getHighLimit();
            LOGGER.trace("Angle exceeds high limit. Requested: {}, Using: {}", argAngle, newAngle);
            argAngle = newAngle;
        }
        if(argAngle < getLowLimit()){
            int newAngle = getLowLimit();
            LOGGER.trace("Angle exceeds low limit. Requested: {}, Using: {}", argAngle, newAngle);
            argAngle = newAngle;
        }
        double offset = getMicrosPerDeg() * (argAngle + getCenter());
        double mid = (MIN_PULSE + MAX_PULSE) / 2;
        int pulse = (int) Math.round(mid + offset);
        if (pulse < MIN_PULSE) {
            LOGGER.trace("specified angle exceeds minimum, returning minimum instead");
            pulse = MIN_PULSE;
        }
        if (pulse > MAX_PULSE) {
            LOGGER.trace("specified angle exceeds maximum, returning maximum instead");
            pulse = MAX_PULSE;
        }
        LOGGER.trace("Calculating pulse length for servo angle {}: {}\u00B5S", argAngle, pulse);
        return pulse;
    }

I created a fluent builder for the servo implementation object. This makes it nicer to create an instance of servo.

    private Servo getTestServo(){
        return new ServoImpl.Builder()
                .name("TestServo")
                .channel(0)
                .center(0)
                .build();
    }


This method returns the angle adjusted for the center and enforcing the limits. It also uses a helper method to get the number of microseconds per deg of angle for  servo with the range as set. The test case ServoImplTest is a good place to go see how the tests for this method work.

    @Test
    public void testGetPulseLength(){
        Servo servo = getTestServo();

        // TESTING CENTER
        LOGGER.info("Testing CENTER");
        servo.setAngle(0);
        assertPulse(servo, 1500);

This test tests if a servo with no center offset set to angle 0 returns the correct 1500ms pulse. There are test for different angles to test the range of options.

Servo Driver

The servo driver provides the interface between a servo and the actual servo. It has methods to update the servo position and to set the frequency. The implementation of the driver is specific to the Adafruit 16-Channel 12-bit PWM/Servo Driver . It could be reused for another PCA9685 device. 

Set PWMFrequency


    @Override
    public void setPulseFrequency(int frequency) throws IOException {
        LOGGER.info("Setting pwm frequency to {} hz", frequency);
        this.frequency = frequency;
        int prescale = getPreScale(frequency);

        LOGGER.debug("Reading value of Mode 1 register");
        int oldMode1 = device.readRegister(PCA9685Device.MODE1);
        LOGGER.debug("Mode 1 register: {}", Integer.toHexString(oldMode1));

        int newMode1 = (oldMode1 & 0x7F) | PCA9685Device.MODE1_SLEEP;
        LOGGER.debug("Setting sleep bit on Mode 1 register: {}", Integer.toHexString(newMode1));
        device.writeRegister(PCA9685Device.MODE1, (byte)newMode1);

        LOGGER.debug("Writing prescale register with: {}", Integer.toHexString(prescale));
        device.writeRegister(PCA9685Device.PRESCALE, (byte)prescale);

        newMode1 = oldMode1 & ~PCA9685Device.MODE1_SLEEP;
        LOGGER.debug("Writing the old value back to mode1 register with sleep off to start osc again: {}", Integer.toHexString(newMode1));
        device.writeRegister(PCA9685Device.MODE1, (byte)(newMode1));
        // wait for oscillator to restart
        sleep(50);
        newMode1 = oldMode1 | PCA9685Device.MODE1_RESTART;
        LOGGER.debug("Setting restart bit: {}", Integer.toHexString(newMode1));
        device.writeRegister(PCA9685Device.MODE1, (byte)newMode1);
    }

The set frequency method writes the value to the prescale register then restarts the PCA9685 oscillator to send Pulse Width Modulation (PWM) signals to the servos. Unit tests are provided to test the values sent to the register are correct. The set frequency calls the method getPreScale(frequency) to get the actual value. I've talked about that in a previous post. It's work looking at to see how the value is calculated. http://www.margic.com/2015/03/java-pca9685-pwm-driver-first-servo.html it's worth checking that out as it includes details on how to add a necessary correction factor that deviates from the PCA9685 datasheet.

Update Servo

This is the main method used to set a single servo's position in the java driver.

    @Override
    public void updateServo(Servo servo) throws IOException {
        // update cache first
        cacheServo(servo);
        int servoChannel = servo.getChannel();
        int pulseLength = servo.getPulseLength(servo.getAngle());

        // calc num counts for ms
        long count = Math.round(pulseLength * RESOLUTION / ((double)1 / (double)getPulseFrequency()) / (double)1000000);

        LOGGER.debug("Updating servo position: {}, count: {}", servo.toString(), count);

        byte[] offBytes = ByteUtils.get2ByteInt((int)count);
        device.writeRegister(getRegisterForChannel(servoChannel, Register.ON_LOW), (byte) 0x00);
        device.writeRegister(getRegisterForChannel(servoChannel, Register.ON_HIGH), (byte) 0x00);
        device.writeRegister(getRegisterForChannel(servoChannel, Register.OFF_LOW), offBytes[ByteUtils.LOW_BYTE]);
        device.writeRegister(getRegisterForChannel(servoChannel, Register.OFF_HIGH), offBytes[ByteUtils.HIGH_BYTE]);
    }

The updateServo method accepts a servo object. This was a driver behind creating the new servo interface for this update. It allows removing the dependency directly on the implementation. This method has to determine the number of counts out of the max resolution of 4096 that apply to a given servo position. For example if the desired PWM duty cycle is 50% (on for 50% of the time) at 50Hz refresh that would be 2048 counts. It's always 2048 for 50% cycle, however the actual time for the cycle depends on frequency. At 50Hz 100% cycle is 20ms in length at 100Hz it would be half that at 10ms. Therefore I measure servo PWM in length not duty cycle. As servo center should be 1.5ms regardless of frequency.

In the method I have a rather clumsy calculation for pulse length. I'll fix it later. However once again test case are provided to validate it does set the correct values to the PCA9685 registers.

2 comments: