Tuesday, April 21, 2015

AVR PWM Capture of 6 Channel Spektrum RC Receiver

I use the Spektrum receiver out of convienience as I have a Spektrum radio handy. The way the receiver outputs the channels will effect the way I multiplex the signals. When looking at the outputs of the receiver in the Saleae Logic analyzer it can be seen that the channels are not sent at the same time. They are sent sequentially. My device was set up to monitor the channels in numeric order. Channel 0 on the receiver mapping to channel 0 on the analyzer.
Spektrum AR610 Receiver

See the closeup of the receiver to the right. The channels mappings by default in the first setup are:

Channel 0 - THRO
Channel 1 - AILE
Channel 2 - ELEV
Channel 3 - RUDD
Channel 4 - GEAR
Channel 5 - AUX1


Spektrum Receiver Output
Looking at the output of those channels in the logic analyzer we can see how this particular Spektrum receiver outputs the PWM pulses. It would be interesting to test the output of additional receivers to determine how they output. As this is the receiver I will install in my PiHexJ robot there is not incentive to do that now.

At the end of my previous post Raspberry Pi Based Java Hex Robot: PiHexJ Radio Control Intro - The Control Event I has some open questions regarding the processing of the receiver signals by the AVR microprocessor. I outlined my design for multiplexing the signals to take advantage of the AVR microprocessors input capture feature for PWM decoding. I had concerns about how this would operate.

Multiplexing the PWM signals from the AR610 Spektrum Receiver

The test bed is shown in the picture below. I am using a 74HC4051 8 Channel Multiplexer to multiplex the Spektrum AR610 6 channel receiver. The questions to address are will there be issues with overlapping signals causing incorrect results on switching and what will the latency be.

Test Setup for RC Receiver Multiplexing
74HC4051 Pin Description
The multiplexer uses uses three pins to switch the multiplexer input to the the output pin (Z). The input pins high low like a binary counter. By using pins 0-2 on PORTD on PORTD I can use a counter to count between the input channels. The device supports 8 channels I am using 6. By setting the PORTD ouput I can switch the input to the input capture pin (14 - ICP1)

74HC4051 Function Table
AVR Mega 328 pin diagram
I am using PORTD pins on my AVR microprocessor to drive the multiplexer. PORTD is convenient as it still allows the programmer to connected and keeps the I2C (SDA and SCL) pins free.

My Pin mappings are:
AVRMultiplexer
PD0S0
PD1S1
PD2S2
PD3E
PB0Z
And PD4 is used to connect to the raspberry pi to signal when the control input has changed.

For the AVR firmware C code check out my github the file is in the hexfirmware repository https://github.com/margic/hexfirmware/blob/master/Firmware/firmware.c rather than the main PiHexJ repository. The AVR is initialized with the input capture interrupt enabled. The following code is triggered when the ICR interrupt is fired. It grabs the value from the timer register and stores it in an array channels[]. The index, plex, is then updated. The value is compared and if changed the update count is updated. I do this so I don't trigger updates to the raspberry pi if no input changed have occurred. PORTD pin PIEVENT is turned on to signal the pi to read the channels via I2C as per my design. PIEVENT corresponds to pin PD4. 

Finally the multiplexer is stepped to the next input.

ISR(TIMER1_CAPT_vect){
	// Input capture interrupt handler
	// first time is triggered in leading edge
	if(PINB & (0x01)){ // pin high 
		// clear counter and set to trailing edge
		TCNT1 = 0;
		TCCR1B &= ~(1<<ICES1);
	}else{
		// get counter from ICR1
		// reset to look for leading edge
		uint_fast16_t counter = ICR1 + 47; // add some counts for processor over head //
		if(channels[plex] != counter){
			updatedCount += 1;
		}
		channels[plex] = counter; 
		
		if(plex == NUMCHANNELS - 1){
			if(updatedCount != 0){
				PORTD |= (1 << PIEVENT);
			}
			updatedCount = 0;
			plex = 0;
		}else{
			plex += 1;
		}
		switchMultiplex();
		TCCR1B |= (1<<ICES1);  
	}
}

void switchMultiplex(){
	uint8_t temp = PORTD & 0xF0;
	PORTD = temp | plex;
}
The code works as intended. The following diagram shows the output on channel 6 of the logic analyzer. 
Multiplexed Signal at AVR ICR Pin Channel 6
While it works it does take over 51 ms to get all the channels switched to the avr. This is due to the way the capture has to wait for the leading edge of the signals. I experimented with changing the order of the input to the multiplexer to optimize the way the AVR reads the signals. This looked promising when reordering the channels in the Saleae Logic Analyzer.

Optimized Receiver Channels
When optimized this way the total time is reduced to 10.5 ms. This looked promising so I ran a test using the multiplexer to see if it could catch the transitions. In theory it should as the switch time of the multiplexer is rated at 5 nano seconds with a max of 12 nano seconds. In practice however....

Failed Multiplexing Channel 6
In practice it did not work. The processor overhead is too much and channel six above shows that the pulses are not being multiplexed correctly. I tried ordering the input channels staggered to avoid this and still gain. This final output blow looks more promising.


The output of all 6 channels are being sent to the AVR and measured. Shown on channel 6 above. The total time is 31.7ms. While not as quick as the optimal, it does work and it is still faster than over 50ms. This will be the layout I used for the initial RC control of the robot.

Conclusion

There will be additional latency as the Raspberry Pi will read the values from the AVR controller to determine the values the receiver is receiving from the radio. In other applications this would not be acceptable. However in this application a estimated 50ms latency before the robot responds should negligible. This can be revisited if it proves to be a problem.

Next Steps

The next tasks are to make the Raspberry Pi respond to the AVR signal (PD4 going high) and have it request the values via I2C. I am currently developing the simulator file that will test the AVR code. I will publish it when complete.

Thursday, April 16, 2015

PiHexJ Radio Control Intro - The Control Event


Adding radio control to the PiHexJ robot is not straight forward. In a simple radio controlled object there is a 1:1 relationship between the movement of a control stick to the movement of a control servo. Push the stick forward servo moves forward. For the walking robot moving the stick forward should result in a coordinated/animated movement of up to 18 servos on the robot.

My challenge is to interpret that single input and turn it into a series of motor movements. In this post I'll look at the conceptual design for capturing the control input. I designed PiHexJ around the concept of an event bus to allow for multiple sources to generate input events the robot should react to. This is implemented in Java on the Raspberry Pi with a Guava Eventbus and Camel used to integration with the bus and other elements.

EventBus Logical
This design supports the concept of a control event being created in the RC module and posting that event to the even bus. A controller should then process that event and "something" should happen. So how to generate a control event for the event bus?

Creating a Control Event

The Raspberry Pi is not going to be very effective accurately timing the PWM signals from the receiver. I needed something more reliable. I had an AVR micro controller I expected would be suitable for the job. I intended to use this read receiver and sensor inputs and provide these to the Raspberry Pi for processing. The best way to do this would be to use a time capture feature on the AVR. There is a good document providing an example of this available from Atmel Using Timer Capture to Measure PWM Duty Cycle. This method is an excellent way as it allows the PWM to be captured using interrupts minimizing the overhead on the device. However there is a problem. The AVR I am using only has a single timer available. This means it's impossible to handle decoding the 6 channels of Radio Control receiver I am using.

The PiHexJ does have one advantage over a typical radio controlled device. Latency is not a problem. I have flown radio controlled aircraft, planes and helicopters. In those applications it's essential that the latency of the radio control is kept to a minimum as the aircraft should respond immediately to input. This is less of a problem in the robot. Each response to a control input is a series of motions that take some time to complete. Therefore latency should not be so perceptible. That provided the option of using a multiplexor between the receiver and the AVR micro controller as shown in the diagram below.

I intend to sample each channel then step the multiplexer to the next input and sample the pulse from it. RC PWM is easier to read as duty cycle is not important only the duration of the pulse counts. Pulse frequency is approximately 50hz so in theory it should be possible to sample receiver output somewhere in the region of 100-200ms.

Design of PiHexJ Radio Control Components

Building the Test Bed

Prototyping the Radio Control
To determine how the capture will operate I have started building out the RC components as shown in the photographs. The first shows the overview of the Raspberry Pi (Top Right), the PCA9685 device (green led), the Radio Transmitter and receiver (left).

The second picture shows the initial layout of the micro controller (28pin IC) and the multiplexer (16pin IC) with the receiver connected via green signal wires.

Multiplexing the receiver signals

The next steps are:

  • Add power and add the feed to leds to show multiplexer channel selection. 
  • Add GPIO event signal from AVR to Raspberry Pi to signal a sample is ready
  • Add mechanism to read samples from AVR with Raspberry PI
  • Generate the control event in the Raspberry Pi.

Questions to Address

The purpose of the testing is to determine if multiplexing can operate without significant latency?

Can the multiplexer switching be monitored via LEDs?

How can the AVR handle multiplexed PWM timer capture. Can I detect if the multiplexor switches onto a channel mid way during an input pulse?

Wednesday, April 15, 2015

Optimizing Java PCA9685 driver using Saleae Logic 8 Analyzer Part 2

PiHexJ Java PCA9685 Driver

At the end of the last post I had changed the driver to update all the servo channel registers in one sequential write to the I2C Bus. Instead of writing <<address>><<value>><<address>><<value>> I changed it to write <<address>><<value>><<value>>

In the last test I was writing address = 0 and writing all the registers. The PCA9685 has a the auto increment flag bit 5 on mode 1 register. I was not sure if when set I had to start at 0 and increment all registers. I did not know if the register auto increment was reset on a I2C stop command.



I reworked the driver to allow this test to allow the driver to update starting from the first updated servo from the servo update buffer. The PCA9685 did works as expected. I can write servos starting from any channel and write the registers in a sequence. This mean updating the driver to optimize this write pattern. In my initial test it took 3ms to update a single servo without register auto increment. It took 6.5ms to write all registers using auto increment. This is a much faster write rate and the pay is when writing 3 or more servos.

Optimized Single Servo Update

With the optimized driver starting from the first updated servo the times are improved further. The screenshot before shows the write of a single servo.
Update single servo 0.56ms
The time for a single update is 0.56ms. This is way faster that both the 6.5ms sequential write or the 3ms for a single. With this scenario sequential write with register auto increment is always faster.

Optimized Multiple Servo Update

The following screen is showing updating 3 servos. 
3 Servo sequential servo update 1.275ms

Writing 3 servos took 1.275ms. These were three adjacent servo channels 0,1&2. It would take longer if writing 0 and 16 as the sequential write has to start at the lowest channel and auto increment over each channel to the last channel.

Changes to the Driver

With the results of the tests I made the following:

  1. Added UpdateSequential flag - It is unlikely to mix the modes between single servo updates vs sequential servo update. It made sense to make this a constructor on the servo driver. This means the driver update mode is set when the driver is created.
  2. Used the buffer to buffer update events - the update device method checks the first update and uses that channel to set the first register address. It then loops through the updates taking either the new settings in the update buffer or uses a cached value to write the current value back to the register to avoid writing empty values when sequentially writing registers.
  3. Added test for the new writing methods

I have to have to add tests for single write operations to ensure I haven't broken those operations even thought I would recommend using the optimized sequential write mode. I have yet to see a Java PCA9685 driver that works this way.





Sunday, April 12, 2015

Optimizing Java PCA9685 driver using Saleae Logic 8 Analyzer Part 1

In my previous post Saleae Logic 8 to Monitor Servo PWM and I2C to PCA9685 I was able to verify my servo updates were performing as expected by monitoring both the I2C traffic and the resulting PWM pulses on the servo channels. The was one observation that gave cause for concern was the time it took to update to update a single servo. It takes 4 register updates to change the servo PWM pulse. That means 4 I2C updates passing the device address, register address and value to write in the register.

In the logic analyzer screenshot below the green and yellow channels show the I2C traffic for the single servo update. The duration is annotated in the window on the right. The duration is approx 3ms. This doesn't sound a lot. However a typical leg move will require updates to at least 3 servos and possibly upto  9. That means a min of 18ms for all servos to be updated. Sequencing a smooth motion with a 18ms+ latency will not be effective.

Time annotation showing duration of 1 servo update
The PCA9685 data sheet describes bit 5 of the MODE1 register enables auto-increment.

MODE1 Register

The foot note describes the following...
When the Auto Increment flag is set, AI = 1, the Control register is automatically incremented after a read or write. This allows the user to program the registers sequentially.

I wanted to see the effect of using the auto increment function by setting this flag. The datasheet provides an example of using this with the flag clear.
Write all registers
This shows setting the device address, followed by the registers starting with register 0 to set auto increment. I ran a test using this approach writing all registers up to register 69 that corresponds to the 16th servo channel.

I ran a test setting all the registers from register 0 as per the datasheet example to evaluate the time take. The screen shot below from the Saleae logic analyzer shows the I2C traffic. The time for writing all 69 registers is 6.5ms compared to 3ms for 4 registers individually. This is a significant improvement and will be way more effective for creating smoother motions when animating the servos to perform leg movements.


Time annotation showing duration of sequential 16 servo update
The next test I will try to further optimize the writes is to determine if I have to start at register zero when using auto increment. The datasheet is not specific about this but does hint at the possibility as it mentions auto increment also works on registers 250 - 254 before rolling over to register 0. What is not clear is if the I start at servo 0 register, register 6 and right some registers what will happen.

The Salae logic analyzer will help determine the what will happen to the servo outputs. I will post an update once I have the test results. I have checked in the code used to test this use case. It obviously will need further updates once the optimization testing is done.


Tuesday, April 7, 2015

Saleae Logic 8 to Monitor Servo PWM and I2C to PCA9685


I started working on sending multiple signals to multiple servo channels. The PCA9685 is a 16 Channel PWM controller. As I need to work with 18 Channels for my PiHexJ robot at some point I have to add support for more than one PCA9685 device. I have two ready to hook up. That gives me 32 channels enough for the all 18 leg servos and plenty left over for additional controls.

In order to get a better view of what's going on at the I2C bus level I need a better analyzer. I decided on a Saleae Logic 8. I have a number of up coming use cases that I want to use this for including;
  • Analyzing the I2C bus when addressing more than 1 device
  • PWM measurement in response to servo update commands
  • Measuring sensor output and ADC output
  • RC receiver decoding that I will be working on shortly.
The Saleae device is tiny. In the picture below you can see the size relative to my breadboard. It connects to my Mac via USB and comes with 8 input channels. I have hooked up the device as follows
  • channels 0-4 to corresponding servo PWM signal output channels 0 -4
  • channel 4 to I2C SDA
  • channel 5 to I2C SCL
To test out the Saleae I am going to test updating a servo output. Before the capture I update the servo to -90 but posting {"channel":0,"angle":-90} to my rest api on PiHexJ. I create the rest api to make it easier to send these kind of commands for the PiHexJ robot controller to handle.


Analyzing servo output with Saleae Logic 8 
I2C signals a start by pulling SDA low. This is the trigger. I set the trigger in the Saleae Logic application on the falling edge of SDA and started the capture so I could see the I2C bus messages and the resulting change to the signal output on the servo channel. Once set I post {"channel":0,"angle":90} to the rest interface PiHexJ will send the I2C instruction to the PCA9685 controller to set the PWM output to the max. The pulse width should change from 1ms to 2ms

Capture of I2C and Servo PWM Channel 0
The Saleae device can capture up to 8 channels. In this example I am only showing 3 channels. On the left of the screen shot you can see the channel labels. The logic application includes protocol analyzers for some common protocols including I2C. I have added the I2C protocol analyzer to my capture with SDA and SCL marked appropriately.

Channel 0 is my servo channel shown in blue. As you can see the blue channel has pulses regularly with the I2C data sent in the yellow and green channels in the middle of the screen. Lets zoom in to see the change to PCA9685 PWM output as a result of the I2C signals.

PWM updated via I2C
On the Servo0 channel (blue) you can see the the signal has changed from length 1ms to 2ms. The Saleae Logic application has a useful measure annotation you can put on the trace to measure the width of the signal. I was looking for 1ms and 2ms. It's pretty close at 1.025ms and 2.046.

Now lets zoom in even further and look at the I2C signals:

I2C Signals
My code is far from ideal as it writes registers one by one. I can optimize that later with the operation that writes sequential registers. However we can see the signals correspond to the code.
    @Override
    public void updateServo(ServoUpdateEvent servoUpdate) throws IOException {
        if (servoUpdate.getServo().getServoConfig().getChannel() > 15) {
            log.warn("Haven't implemented channels > 15");
            return;
        }
        // update cache first
        if (isMoved(servoUpdate)) {
            // only send update to servo if its position has actually moved.
            cacheServo(servoUpdate);
            int servoChannel = servoUpdate.getServo().getServoConfig().getChannel();
            int pulseLength = servoUpdate.getServo().getPulseLength(servoUpdate.getAngle());

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

            log.debug("Updating servo position: {}, count: {}", servoUpdate.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]);
        }
    }
I currently start servo pulses at 0 so I write 0 to both ON low and high registers. This is not smart I should stagger this to spread the power draw more evenly.

Looking at the debug from my PiHexJ code I can see the update servo log entry:
DEBUG com.margic.adafruitpwm.AdafruitServoDriver - Updating servo position: com.margic.pihex.event.ServoUpdateEvent@d71bd3[servo=com.margic.pihex.ServoImpl@1342314[servoConfig=com.margic.pihex.model.ServoConfig@149ca71[name=Leg 0 Coxa,channel=0,range=180,center=0,startAngle=0,lowlimit=-90,highLimit=90]],angle=90], count: 410

I currently start servo pulses at 0 so I write 0 to both ON low and high registers. This is not smart I should stagger this to spread the power draw more evenly. My code calculates for a default calibration a +90 deg angle (or max right angle) should be 410 counts of the 4096 total counts for a 20ms duration. Where 4096 is the servo drivers resolution. Therefore on is 0 off is 410.
410 is 0000000110011010 in binary
or

high byte 0000 0001 = 0x01
low byte 1001 1010 = 0x9A

There are four interactions corresponding to the 4 writes in my ServoDriver class. My code writes correspond to following: (see PCA9685 Datasheet for reference)
  • 0x00 to register 0x06 (LED0_ON_L)
  • 0x00 to register 0x07 (LED0_ON_H)
  • 0x9A to register 0x08 (LED0_OFF_L)
  • 0x01 to register 0x09 (LED0_OFF_H)
Lets look at that third write even closer to see the signaling of register and value


The Saleae Logic application's protocol analyzer has done the hard work of decoding the I2C values for us. We can see my application wrote 0x80, the register address and 0x9A the register value. In addition to automatically adding the labels in blue above the SDA channel it also has a protocol output window. I have it set to show from the start of the interaction to the end. This window shows all the values written for my PCA9685 I2C writes.

In conclusion I think the Saleae Logic 8 device will be extremely beneficial for my PiHexJ Raspberry Pi Based Hex Robot. Over the next few weeks I'm sure I'll be posting more use cases. I'm looking forward to the RC radio receive signal decoding, multiplexing and timing in the AVR microcontroller.

Monday, April 6, 2015

Running a Camel Route at Startup once only to load and process PiHexJ's servo configs

Servo Configuration File Loading

The servo calibration data is stored in config files as JSON representation. In the previous post I showed how I capture the config via a REST interface. To keep this simple each servo has it's own file name servo-{channel}.conf eg; servo-0.conf. At some point I'll  update to aggregate the configuration into a single file. Since it makes no difference to the operation of the robot, the simple approach will work for now.

Servo config files are stored in a folder specified by a property. What I wanted is a camel route to load the files and set the configuration of each servo when the PiHexJ main app starts up. The route should run once and only once. After the app is started the config is managed via the rest interface. If the configuration is updated a new conf file is written to persist the settings for then next startup.

servo-0.conf has a single record in json format. This file stores the settings that each servo needs to calibrate for differences in servo models and installation position.
{"channel":1,"range":180,"center":-5,"lowLimit":-80,"highLimit":90,"name":"Leg 0 Femur"}

The camel route to load these files is fairly straight forward

from("file:{{config:com.margic.pihex.servo.conf}}?noop=true&charset=UTF-8&include=.*.conf")
        .routeId("loadServoConfig")
        .autoStartup(false)
        .log(LoggingLevel.INFO, "loading config file ${in.header.CamelFileName}")
        .unmarshal().json(JsonLibrary.Jackson, ServoConfig.class)
        .to("bean:controller")
        .choice()
            .when(exchangeProperty("CamelBatchComplete").isEqualTo(true))
            .log(LoggingLevel.INFO, "Loaded all servo conf files stopping loader")
            .process(new StopProcessor())
        .endChoice();

Notice the route has .autoStartup(false) this prevents this route from starting when Camel starts up. We want to control when this route starts to ensure the system is ready before loading the configuration files.
The route reads all the files in the folder and loads them into the controller. The controller is a bean injected into the camel registry using my guice jndi binding mentioned in an earlier post. This allows me to send the message .to("bean:controller"). At this point in the flow the body of the camel message is a ServoConfig object. Camel's bean component using bean binding to automatically invoke the method on the bean that matches the argument type of a ServoConfig object and processes the config. Bean binding makes this very simple.

Stopping the route when finished loading conf files

The Camel file component is a batch consumer. This means it supports polling multiple messages. In my case camel reads all the files and creates a message for each. The batch consumer uses the following properties to signal the state of the batch.

Exchange Properties

The following properties is set on the Exchange for each Exchange polled in the same batch.
Property
Description
CamelBatchSize
The total number of Exchanges that was polled in this batch.
CamelBatchIndex
The current index of the batch. Starts from 0.
CamelBatchComplete
A boolean indicating the last Exchange in the batch. Is only true for the last entry.
I use these in the choice in my route
        .choice()
            .when(exchangeProperty("CamelBatchComplete").isEqualTo(true))
            .log(LoggingLevel.INFO, "Loaded all servo conf files stopping loader")
            .process(new StopProcessor())
        .endChoice();
The choice checks the property CamelBatchComplete. this property will be true when the message is the last message (last file). We can then instruct the batch to stop. This needs to be done in a separate thread to allow a controlled shutdown. I use a new processor as per the Camel examples.
    class StopProcessor implements Processor{
        Thread stop;
        @Override
        public void process(final Exchange exchange) throws Exception {
            // stop this route using a thread that will stop
            // this route gracefully while we are still running
            if (stop == null) {
                stop = new Thread() {
                    @Override
                    public void run() {
                        try {
                            exchange.getContext().stopRoute("loadServoConfig");
                        } catch (Exception e) {}
                    }
                };
            }
            stop.start();
        }
    }

When this thread runs it grabs the context from the last message and issues a stop route command with the id of the route. Stopping the route ensure it won't run again if the config files change while the robot is running.

Starting the Camel Route after Startup

Now I have a way to load the files and stop the route I need a way to trigger the startup of the route after the camel context is fully started. Camel provides an interface StartupListener to provide a event handler on camel startup.

In PiHexJ I used an eventbus to handle events in the application. So to trigger the startup event I added a listener to my custom camel context that implements StartupListener and posts an event to the eventbus.

public class StartupListener implements org.apache.camel.StartupListener {
    private static final Logger log = LoggerFactory.getLogger(StartupListener.class);

    @Override
    public void onCamelContextStarted(CamelContext context, boolean alreadyStarted) throws Exception {
        log.info("Camel startup listener, alreadyStarted: {}", alreadyStarted);
        if(!alreadyStarted){
            log.debug("Initial Startup Trigger Event on Event Bus");
            EventBus eventBus = context.getRegistry().lookupByNameAndType("eventBus", EventBus.class);
            if(eventBus == null){
                throw new Exception("Failed to lookup eventbus in camel registry. Can't initialize properly");
            }
            eventBus.post(new StartupEvent());
        }
    }
}

Now I have an event I use in another route to start the actual servo calibration conf file loader. Having this intermediate route will allow me to do multiple startup actions at a later point if required.
These two routes look like this:

from("guava-eventbus:{{config:com.margic.pihex.camel.eventBusName}}?listenerInterface=com.margic.pihex.camel.route.EventBusEvents")
        .routeId("eventBusRoute")
        .choice()
            .when(body().isInstanceOf(Servo.class))
                .to("seda:updateServo")
            .when(body().isInstanceOf(ControlEvent.class))
                .to("bean:controller?method=handleControlEvent")
            .when(body().isInstanceOf(StartupEvent.class))
                .to("direct:handleStartupEvent")
        .endChoice();

from("direct:handleStartupEvent")
        .routeId("handleStartupEventRoute")
        .log(LoggingLevel.INFO, "Starting loadServoConfigRoute")
        .to("controlbus:route?routeId=loadServoConfig&action=start");

Camel provides a controlbus component that allows control over routes. In my example I receive the event on the event bus and when that event is a StartupEvent I send the message to the handleStartupEvent route. All that route does is sent the message to the control bus to start the loadServoConfig route. I could send the message directly to the control bus but as mentioned the handleStartupEvent route will allow me to add additional startup steps at a later point.

Now we have a way to start a route on startup and shut it down once it's run.

[Thread-0] INFO com.margic.pihex.camel.context.CustomCamelContext - Total 10 routes, of which 9 is started.
[Thread-0] INFO com.margic.pihex.camel.context.CustomCamelContext - Apache Camel 2.15.0 (CamelContext: camel-1) started in 0.664 seconds
[pool-1-thread-1] INFO handleStartupEventRoute - Starting loadServoConfigRoute
[pool-1-thread-1] DEBUG com.margic.pihex.camel.context.CustomCamelContext - Warming up route id: loadServoConfig having autoStartup=true
[pool-1-thread-1] DEBUG com.margic.pihex.camel.context.CustomCamelContext - Route: loadServoConfig >>> EventDrivenConsumerRoute[Endpoint[file:///Users/paulcrofts/conf/?charset=UTF-8&include=.*.conf&noop=true] -> Pipeline[[Channel[Log(loadServoConfig)[loading config file ${in.header.CamelFileName}]], Channel[Unmarshal[org.apache.camel.component.jackson.JacksonDataFormat@467b7308]], Channel[sendTo(Endpoint[bean://controller])], Channel[sendTo(Endpoint[bean://controller])], Channel[choice{when Filter[if: exchangeProperty{CamelBatchComplete} == true do: Pipeline[[Channel[Log(loadServoConfig)[Loaded all servo conf files stopping loader]], Channel[DelegateSync[com.margic.pihex.camel.route.StartupRouteBuilder$StopProcessor@2c2734a9]]]]]}]]]]
[pool-1-thread-1] DEBUG com.margic.pihex.camel.context.CustomCamelContext - Starting consumer (order: 1010) on route: loadServoConfig
[pool-1-thread-1] INFO com.margic.pihex.camel.context.CustomCamelContext - Route: loadServoConfig started and consuming from: Endpoint[file:///Users/paulcrofts/conf/?charset=UTF-8&include=.*.conf&noop=true]
[pool-1-thread-1] INFO org.apache.camel.component.controlbus.ControlBusProducer - ControlBus task done [start route loadServoConfig] with result -> void
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] INFO loadServoConfig - loading config file servo-0.conf
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.pihex.ServoImpl - µS/deg: 5.555555555555555 for range: 180
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.adafruitpwm.AdafruitServoDriver - Updating servo position: com.margic.pihex.ServoImpl@7600c818[angle=0,servoConfig=com.margic.pihex.model.ServoConfig@31db1724[name=Leg 0 Coxa,channel=0,range=180,center=5,lowlimit=-80,highLimit=80]], count: 313
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] INFO loadServoConfig - loading config file servo-1.conf
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.pihex.ServoImpl - µS/deg: 5.555555555555555 for range: 180
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.adafruitpwm.AdafruitServoDriver - Updating servo position: com.margic.pihex.ServoImpl@1cbcac67[angle=0,servoConfig=com.margic.pihex.model.ServoConfig@7aea0edd[name=Leg 0 Femur,channel=1,range=180,center=-5,lowlimit=-80,highLimit=90]], count: 301
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] INFO loadServoConfig - loading config file servo-2.conf
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.pihex.ServoImpl - µS/deg: 5.555555555555555 for range: 180
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] DEBUG com.margic.adafruitpwm.AdafruitServoDriver - Updating servo position: com.margic.pihex.ServoImpl@37a2866f[angle=0,servoConfig=com.margic.pihex.model.ServoConfig@44a3e854[name=Leg 0 Tibia,channel=2,range=180,center=10,lowlimit=-90,highLimit=90]], count: 319
[Camel (camel-1) thread #1 - file:///Users/paulcrofts/conf/] INFO loadServoConfig - Loaded all servo conf files stopping loader
[Thread-11] INFO org.apache.camel.impl.DefaultShutdownStrategy - Starting to graceful shutdown 1 routes (timeout 300 seconds)
[Camel (camel-1) thread #2 - ShutdownTask] INFO org.apache.camel.impl.DefaultShutdownStrategy - Route: loadServoConfig shutdown complete, was consuming from: Endpoint[file:///Users/paulcrofts/conf/?charset=UTF-8&include=.*.conf&noop=true]
[Thread-11] INFO org.apache.camel.impl.DefaultShutdownStrategy - Graceful shutdown of 1 routes completed in 0 seconds
[Thread-11] INFO com.margic.pihex.camel.context.CustomCamelContext - Route: loadServoConfig is stopped, was consuming from: Endpoint[file:///Users/paulcrofts/conf/?charset=UTF-8&include=.*.conf&noop=true]

As you can see the Camel context starts up with 9 of the 10 routes running. Then the event is triggered starting the load route. Each conf is loaded. I have only 3 in this example. The servo configurations are loaded and the servo configurations are updated. Finally the loader route is gracefully shutdown.

Sunday, April 5, 2015

Using Camel Rest DSL to Create REST API

It used to be pretty fiddly to create a rest interface in camel. As of Camel 2.14 a new Rest DSL is available for creating Rest API Endpoints in Camel. I wanted to create rest endpoints to manage sending both config and control data to my PiHex robot.

My design uses an eventbus to handle different events from different sources. For example; updating servo calibration data via rest, loading config from persistent storage receiving controller input via rest or via radio control. There will be more.

At this point I want to focus on adding the rest support. In particular for getting and updating the servo config model object. I've currently kept this very simple. Each servo has it's one config url http://mypi:port/servoconfig/{channel} where 'servoconfig' is the entity and id is the numeric id of the servo channel.

@XmlRootElement
public class ServoConfig {

    private static final Logger log = LoggerFactory.getLogger(ServoConfig.class);

    public static final int DEFAULT_RANGE = 180;
    public static final int MIN_PULSE = 1000;
    public static final int MAX_PULSE = 2000;

    private int channel;
    // range 180 -90 = 1000 90 = 2000 0 = 1500 0-90 = 500
    private int range;
    private int center;
    private int lowLimit = Integer.MIN_VALUE; // can't initialize to 0, 0 is a valid limit
    private int highLimit = Integer.MAX_VALUE; // can't initialize to 0, 0 is a valid limit
    private String name;

    public String getName() {...}

    public void setName(String name) {...}

    public int getChannel() {...}

    public void setChannel(int channel) {...}

    public int getRange() {...}

    public void setRange(int range) {...}

    public int getCenter() {...}

    public void setCenter(int center) {...}

    public int getLowLimit() {...}

    public void setLowLimit(int lowLimit) {...}

    public int getHighLimit() {...}

    public void setHighLimit(int highLimit) {...}

The model class ServoConfig is a basic pojo, nothing fancy just annotated with '@XmlRootElement'. I want to be able to view the current config of a servo (GET) and update the config (PUT). These are the only two methods I need. To create the route I am using Jetty for the Http Server and Jackson for the JSON support. To add these we need to add the following to the pom.xml.

        
            org.apache.camel
            camel-jetty
            ${camel.version}
        
        
            org.apache.camel
            camel-jackson
            ${camel.version}
        
Of course the version has to be 2.14 or later. I am using 2.15.0.

Now we can create a rest endpoint in camel. My endpoints are super simple for the robot I have the endpoints for the ServoConfig in a single camel route.

public class ServoConfigRouteBuilder extends RouteBuilder {

    @Override
    public void configure() throws Exception {
        getContext().getTypeConverterRegistry().addTypeConverter(ServoConfig.class, Servo.class, new ServoConfigTypeConverter());
        restConfiguration().component("jetty").host("{{config:com.margic.pihex.api.address}}").port("{{config:com.margic.pihex.api.port}}").bindingMode(RestBindingMode.auto);

        rest("/servoconfig/")
                .get("/{channel}")
                .outType(ServoConfig.class)
                .to("direct:getServoConfig")
                .put("/{channel}")
                .consumes("application/json")
                .type(ServoConfig.class)
                .to("direct:putServoConfig");

        from("direct:getServoConfig")
                .routeId("getServoConfig")
                .setBody(header("channel"))
                .to("bean:controller")
                .convertBodyTo(ServoConfig.class);

        // writes servo calibration to file one per servo for now. will consolidate later
        from("direct:putServoConfig")
                .routeId("putServoConfig")
                .multicast()
                    .to("direct:updateRunningConfig")
                    .to("direct:writeConfigToFile")
                .end()
                .setHeader(Exchange.HTTP_RESPONSE_CODE, constant("204"));

Camel is doing a lot of magic for us here. Rather than the usual DSL endpoint starting with "from" we use "rest" when using the Rest DSL. My GET and PUT http verb rest operations are built using the fluent Rest DLS using .get and .put respectively.

Stepping through the dsl:
rest("/servoconfig/")  // sets the entity path to servoconfig
    .get("/{channel}") // assigns the HTTP get with the path to the variable channel id
    .outType(ServoConfig.class) // sets the return type for the get request
    .to("direct:getServoConfig") // sets the endpoint to send this request to
    .put("/{channel}") // assigns the HTTP put with the path to the variable channel id
    .consumes("application/json") // specify the required media type that matches this route
    .type(ServoConfig.class) // type of the incoming body that the mapper should use to unmarshall the json
    .to("direct:putServoConfig"); // where to send this put request

The Rest DSL does not do the work itself it relies on rest component to implement the endpoints. This is specified in the route builder using the restConfiguration method.

restConfiguration().component("jetty").host("{{config:com.margic.pihex.api.address}}").port("{{config:com.margic.pihex.api.port}}").bindingMode(RestBindingMode.auto);


In this case I am setting the component to Jetty as mentioned and using property placeholders to set the properties for address and port. I like to use placeholders for things like the port it makes it much easier to manage and makes testing easier.

The .bindingMode method sets the way camel will manage binding to and from POJO objects. This is really important. The default is not to bind. You have to specify something here if you want Camel to do the work for you. I use auto mode and specify media types and object types. Don't forget to set something in your route or you'll have to handle this in your route. My objects are so simple I want camel to do most of the heavy lifting for me.

Testing the Camel Rest DSL endpoint.

Testing the endpoint can be done by overriding the endpoints. I personally like to test rest endpoints directly by using an http client as I frequently get issues with media conversions etc and testing with the client is relatively simple. Apache http components have introduced a new fluent http client api that makes this even easier. 

This is my test case to test the GET message.

public class ServoConfigRouteGetBuilderTest extends CustomCamelContextTestSupport {
    private int port;

    @Override
    protected RouteBuilder createRouteBuilder() throws Exception {
        log.info(System.getProperties().toString());
        port = AvailablePortFinder.getNextAvailable(8080);
        setConfigurationProperty("com.margic.pihex.api.port", Integer.toString(port));
        setConfigurationProperty("com.margic.pihex.servo.conf", "${sys:user.dir}/target/test-classes/confwritetest/conf/");
        return new ServoConfigRouteBuilder();
    }
    @Test
    public void testServoConfigGet() throws Exception {
        String content = Request.Get("http://localhost:" + port + "/servoconfig/0")
                .addHeader("Accept", "application/json")
                .execute()
                .returnContent()
                .asString();
        log.info(content);
        JSONAssert.assertEquals("{\"name\":\"Leg 0 Coxa\",\"channel\":0,\"range\":180,\"center\":0,\"lowLimit\":-90,\"highLimit\":90}", content, true);
    }
}

My GET use case is pretty simple I don't need to mock the object being return as there is always a default config for a servo. What is interesting here is the http Request.Get fluent method. The new fluent builder makes it very easy to test the call without to having to worry about creating the client or releasing the resources.

Mapping path params to Camel Messages

In my example above I have the path param {channel} for both my get and put requests. Camel Rest DSL maps the path param to a camel header of the same name as can be seen in the debug below of the incoming message my get route.


Tuesday, March 31, 2015

Binding Guice bindings to Camel Registry

In the previous post I showed how I was using Commons Configuration with Camel property placeholders using Camel properties functions.

In this post I address the second problem I needed to solve. Binding objects that are bound to a guice injector to the camel registry. Again I should point out that Camel has a guice component that will take care of most of this. But I wanted to experiment with a simple camel context. My camel context is very simple it extends DefaultCamelContext and currently only provides one function, that function allows the injection of the commons configuration and a custom registry.
package com.margic.pihex.camel.context;

import com.google.inject.Inject;
import org.apache.camel.component.properties.PropertiesComponent;
import org.apache.camel.impl.DefaultCamelContext;
import org.apache.camel.spi.Registry;
import org.apache.commons.configuration.Configuration;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * Created by paulcrofts on 3/27/15.
 */
public class CustomCamelContext extends DefaultCamelContext {

    private static final Logger log = LoggerFactory.getLogger(CustomCamelContext.class);

    @Inject
    public CustomCamelContext(Registry registry, Configuration config) {
        super(registry);
        log.info("Creating Guice Camel Context");
        PropertiesComponent pc = getComponent("properties", PropertiesComponent.class);
        pc.addFunction(new ConfigurationPropertiesFunction(config));
    }
}

This allows me to inject my registry implementation into the constructor of the camel context. The main binding happens in my JNDI registry implementation. The registry takes the guice injector as an argument and then scans the bindings on the injector for annotations. My code does this manually a better option would be to use matchers but the point is still valid.
    @Inject
    public GuiceRegistry(Injector injector) {
        super();
        this.injector = injector;
        bindGuiceObjects();
    }

    /**
     * method to scan injector bindings to register objects
     * annotated with BindCamelRegistry or names
     */
    private void bindGuiceObjects() {
        if (injector != null) {
            log.debug("Binding guice objects to registry");
            Map< key>, Binding> bindings = injector.getBindings();
            for (Key key : bindings.keySet()) {
                if (key.getAnnotationType() == Named.class) {
                    log.debug("Found binding annotated with named. Should be bound to camel registry.");
                    Object object = injector.getInstance(key);
                    Named named = (Named) key.getAnnotation();
                    String name = named.value();
                    bind(name, object);
                }else {
                    javax.inject.Named annotation = (javax.inject.Named)key.getTypeLiteral().getRawType().getAnnotation(javax.inject.Named.class);
                    if(annotation != null) {
                        log.debug("Found object bound with annotation BindCamelRegistry");
                        String name = annotation.value();
                        if (name != null) {
                            Object object = injector.getInstance(key);
                            bind(name, object);
                        }
                    }
                }
            }
        }
    }


This allows me to bind beans in Guice and have them automatically bound to the camel context. I can do the same with other objects using type matchers. For example RouteBuilders. However for now I am happy to manually configure routes during this stage of development.

The following test validates the binding and shows how easy it is to bind objects.

    @Named("myBean")
    static class TestClass{
        // this is just an empty class bound to name myBean for test
    }

    @Test
    public void testBindCamelRegistryAnnotation(){
        // need injector
        Injector injector = Guice.createInjector(new AbstractModule() {
            @Override
            protected void configure() {
                bind(TestClass.class);
            }
        });

        GuiceRegistry registry = new GuiceRegistry(injector);
        Object object = registry.lookupByName("myBean");
        assertNotNull(object);
        TestClass testClass = registry.lookup("myBean", TestClass.class);
        assertNotNull(testClass);
    }

    @Test
    public void testBindCamelRegistryNamedAnnotation(){
        Injector injector = Guice.createInjector(new AbstractModule() {
            @Override
            protected void configure() {
                bind(String.class).annotatedWith(Names.named("testName")).toInstance("My String Value");
            }
        });

        GuiceRegistry registry = new GuiceRegistry(injector);
        String testString = (String)registry.lookup("testName");
        assertEquals("My String Value", testString);

        testString = registry.lookup("testName", String.class);
        assertEquals("My String Value", testString);
    }


Monday, March 30, 2015

Apache Camel Property Placeholders - Commons Configuration in Raspberry Pi

The last set of changes I made were adding Apache Camel to my PiHexJ code. I added support for a camel context running on the Raspberry Pi. It's fairly easy to get Camel running on the Pi however there is a question I guess I need to answer first.

Why add apache camel to my robot?

There are a number of reasons why I chose to add Camel to the Robot's software stack. Some good reasons, some not so good reasons.  I'll let you judge which is which, probably better than trying to argue one way or another. 
  • A need to integrate for control. I intend to have multiple ways of controlling the robot, via rest interface, by radio control and by the robot sending sensor info to a server to get instructions from the server. 
  • A need to integrate with persistence to read calibration and configuration data.
  • I use camel at my day job and figured it would be good to gain more experience with it using it in a different way.
  • I wanted to see if camel could run well on a raspberry pi.

Running Apache Camel on the Raspberry Pi

Camel can be run in number of different ways. It is most effective when it has a supporting dependency injection framework. In the past I've run it three different ways;
  • OSGI containers using Aries Blueprint
  • With Spring xml
  • Using Guice
  • Directly from java main.
Camel routes can be configured in via xml, however I never have like that approach even though many examples are provided that way. I prefer the Java dsl.
In my raspberry pi application I am using Apache Commons Configuration to abstract my configuration and Guice for dependency injection. I don't have any plans to run an OSGI container on the Pi so that rules our using blueprint. Spring was never on my list as an option so that leaves Guice and running from the Java main.

The obvious choice is Guice. I wanted to be able to use property placeholders in my routes. I like the flexibility of using them. What I wanted is the properties to resolve against the commons configuration since I am using configuration to encapsulate my config. I also needed beans to be bindable via guice and accessible in the camel registry. For example for the camel-guava-eventbus component. I'm using eventbus to provide a bus to allow different components to send events that I can react to and affect the  robots behavior. It gives me a more consistent way to manage the event processing.

Bridging between Commons Configuration, Guice and the Camel Registry

Camel has a guice component. It provides some excellent support. I would suggest it's the correct way to do this however I wanted to experiment with camel a bit more and decided just to roll my own camel context. One of the things I've learned about camel is there are many ways to solve each problem. So many in fact I'd recommend +Scott Cranton book the Apache Camel Developers Cookbook it helps you narrow down approaches to solving the types of real world integration problems you are likely to encounter. Since this is not a real world problem lets crack on (or should that be hack on).

Two problems to solve:
1. Property resolvers using Commons Configuration
2. Binding beans to the camel registry

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.

Saturday, March 21, 2015

Raspberry Pi Java PCA9685 PWM Driver - First servo pulses output

It's now time to turn attention to the servo output channels on the Adafruit PCA9685 device. I haven't completed any of the logic to set the servo angle but wanted to check the Java driver for the Raspberry Pi was working as expected. I needed to send a test Pulse Width Modulation (PWM) signal to one of the out put channels.

Calculating the register settings for a test pulse

A servo pulse is repeated x times a second. The specific frequency is not critical it should be somewhere around 50Hz for a typical servo. What is important is the pulse length. At 50Hz the the entire cycle is 20ms. A servo pulse is some where between 1ms and 2ms. That's between 5% and 10% of the cycle. Given we only have a max of 10% of the PWM to play with any errors are going to be fairly significant.

The PCA9685 uses 2 pairs registers per channel to set the pulse. It has a resolution of 4096bits, thats why it takes a pair of 1byte registers to represent a value with maximum of 4096. The value represents a count of time in the cycle. The first pair represents when the output will be turned on, the second when the output will be turned off.

To determine the frequency a pre scaler is used to scale the PCA9685 internal oscillator from 25MHz to the appropriate value. At 25Mhz 4096 counts would be over way to fast 0.16ms incase your wondering. (Ok I admit thats not actually possible, in fact the actual max of the output is 1Khz) The goal is to get a the modulation frequency to close to 50Hz.

The datasheet provides the formula for setting the prescaler to provide the expected frequency:
prescaleVal = round(oscClock/(4096 * updateRate)) - 1

My Java driver includes a method to set the PWMFrequency

    public int getPreScale(int frequency) throws IOException{
        LOGGER.debug("Get prescale value for frequency {}", frequency);
        
        double prescaleval = CLOCK_FREQUENCY;
        prescaleval /= RESOLUTION;
        prescaleval /= frequency;
        prescaleval -= 1.0;
        LOGGER.debug("Estimated pre-scale {}", prescaleval);
        prescaleval = Math.round(prescaleval + 0.5);

        if(prescaleval > 254){
            throw new IOException("Specified frequency " + frequency + " results in prescale value " + prescaleval + " that exceed limit 254");
        }
        int prescale = (int)prescaleval;
        LOGGER.debug("Final pre-scale {}", prescale);
        return prescale;
    }

To test this I needed to request to send known output to the output channel.
If the resolution is 4096 the time for that amount of counts is 20ms it simply 4096/20 = 204.8 to figure out how many counts represent a 1ms pulse, 1ms being the lowest servo pulse. I happened to round down to 204 or 0xCCh cause it was neater and is insignificant. I hard coded these to set the registers.

        try {
            device.writeRegister(PCA9685Device.LED0_ON_HIGH, (byte) 0x00);
            device.writeRegister(PCA9685Device.LED0_ON_LOW, (byte) 0x00);
            device.writeRegister(PCA9685Device.LED0_OFF_HIGH, (byte) 0x00);
            device.writeRegister(PCA9685Device.LED0_OFF_LOW, (byte)0xCC);
        }catch(IOException ioe){
            LOGGER.error("Error setting pulse", ioe);
        }


The four registers are set for LED0 (channel 0) the first two specify the count when it should turn on. This is set to 0 counts. The pulse will begin at the start of each cycle. 204 is less than 255 so the off high byte is 0 with 204(0xCCh) in the off low byte.
I hooked up the pocket scope to see what the output was. I was a bit surprise to see that the trace did not measure a full second. It's somewhere around 950µs. I wasn't expecting it to be off by that much.

I looked at a lot of sample code before starting my own driver. I had actually written some python code based on some example. I remember seeing one a comment in the code about some correction factor for the PCA9685. Turns out cyanogilvie on github reported an issues to adafruit Frequency accuracy issue

In that issue he reported that a correction factor of 0.9 on the frequency does the trick.

        double correctedFrequency = frequency * 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
        double prescaleval = CLOCK_FREQUENCY;
        prescaleval /= RESOLUTION;
        prescaleval /= correctedFrequency;

With that correction I get the output below. Now it's much closer. It's still not perfect but it's within acceptable tolerances for my purpose now.

I had significant doubts that a $50 scope would be useful enough. I had read other people were looking to use it for measuring servo inputs. At least I can confirm its of some utility for that purpose.

I did not measure how close the cycle is to 50Hz as I mentioned it is not that important. The key factor is the pulse length.

I would recommend adding this correction. I have seen a lot of code out there that does not use this correction. At least I and two others have measured the output can validate it with these parameters.