UltraBorg - Precision servo control with ultrasonic module support

Main Examples

Download and installation instructions can be found on the getting started tab, as well as wiring instructions for the power, servo motors, and ultrasonic modules.
The package of examples can be downloaded as a zip file directly: http://www.piborg.org/downloads/ultraborg/examples.zip
You can also view the code listings at the bottom of this page.

UltraBorg.py

This is the main library for UltraBorg, it simplifies operation from using I²C directly to simply calling simple named functions such as
UB.SetServoPosition1(0.5)
See the scripts below for examples of use, or the library function listings for usage guidance.

ubSequence.py

A simple script which controls the servos automatically based on an example sequence.
This script demonstrates how simple operating UltraBorg can be ^_^
Lines 8 to 17 determine the sequence the rate each servo is moved at, as well as how far to move them.
Run using:
cd ~/ultraborg
./ubSequence.py

ubReadDistances.py

A simple script which reads the ultrasonic distances from all four modules.
Note that this script checks the distances for a reading of 0.
This is because the UltraBorg returns 0 if it either cannot see an ultrasonic module, or if the distance is out of range.
This script uses the filtered ultrasonic readings.
Run using:
cd ~/ultraborg
./ubReadDistances.py

ubServoFromDistances.py

A slightly more advanced script which uses the distance readings to control the servo movement.
Each servo is moved to a position based on how far away the corresponding distance reading is.
This example uses the unfiltered distance readings to get the fastest response to movement.
Lines 8 to 10 determine the distance range used to control the servos in mm.
Run using:
cd ~/ultraborg
./ubServoFromDistances.py

ubGui.py

This script shows how you can make a GUI in Python which controls UltraBorg, as well as providing an easy to use interactive demonstration of the board.

Drag the sliders up and down to move the servos, the current positions are shown below the sliders.
The numbers at the bottom of the GUI are the filtered readings from the ultrasonic modules.
To get the same position as the GUI in a script divide the value shown by 100, e.g. if the GUI shows servo #1 as -35 and you want the same psoition run
UB.SetServoPosition1(-0.35) in your script.
Run using:
cd ~/ultraborg
./ubGui.py
or double-clicking the icon on the desktop.

ubTuningGui.py

This is a complex script which we provide for tuning the servos to operate at full range.

The various buttons provide pop-up text to explain what they do.
See the UltraBorg servo tuning instructions for a more detailed description of how to use this GUI.
The GUI provides an example of how limits can be set from a script.
Run using:
cd ~/ultraborg
./ubTuningGui.py
or double-clicking the icon on the desktop.

Arduino examples

UltraBorgArduino.ino

A simple sketch which controls the servos automatically based on an example sequence.
It also reads the ultrasonic distances from all four modules when it starts and reports them via serial.
This script demonstrates how simple operating UltraBorg can be ^_^
Lines 53 to 61 determine the sequence the rate each servo is moved at, as well as how far to move them.

UltraBorgTuning.ino

This is a larger sketch which we provide for tuning the servos to operate at full range.
The sketch uses the serial input / output to assist in finding and storing the limits of each servo.
See the UltraBorg Arduino servo tuning instructions for a more detailed description of how to use this example.
The sketch provides an example of how limits can be set from the Arduino, as well as how to set the raw PWM level if needed.

User Examples

Please post any examples you would like to share with others on the forum here.

Library Functions

For Raspberry Pi use by creating an instance of the class, call the Init function, then command as desired, e.g.

import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Init()
# User code here, use UB to control the board

For Arduino pull in the header and call the UbInit function, then command as desired, e.g.

#include "UltraBorg.h"
...
	if (!UbInit()) {
		Serial.print("No UltraBorg!");
		digitalWrite(13, HIGH);
	}
	// User code here, use Ub... to control the board

This call may be made either in the setup or loop functions.

Multiple boards can be used when configured with different I²C addresses by creating multiple instances on the Raspberry Pi, e.g.

import UltraBorg
UB1 = UltraBorg.UltraBorg()
UB2 = UltraBorg.UltraBorg()
UB1.i2cAddress = 0x44
UB2.i2cAddress = 0x45
UB1.Init()
UB2.Init()
# User code here, use UB1 and UB2 to control each board separately

On the Arduino the same can be done by changing the I²C address during the code, for example:

#include "UltraBorg.h"
...
	Ubi2cAddress = 0x44
	if (!UbInit()) {
		Serial.print("No UltraBorg at 0x44!");
		digitalWrite(13, HIGH);
	}
	Ubi2cAddress = 0x45
	if (!UbInit()) {
		Serial.print("No UltraBorg at 0x45!");
		digitalWrite(13, HIGH);
	}
	// User code here, use Ub... to control the board, e.g.
	Ubi2cAddress = 0x44
	UbSetServoPosition1(0.0);	// Set servo #1 on board at 0x44 to central
	Ubi2cAddress = 0x45
	UbSetServoPosition1(0.0);	// Set servo #1 on board at 0x45 to central

Be careful if both boards have different servo limits, you may need to re-run UbInit() after changing the I²C address to get the correct servo limits for the board at that address.

The rest of the calls are the same for Arduino as on the Raspberry Pi except for name.
Simply swap UB. for Ub in the examples if you are using an Arduino.
e.g.
UB.SetServoPosition1 on the Raspberry Pi becomes
UbSetServoPosition1 on the Arduino.

On a Raspberry Pi you can get explanations of the functions available by call the Help function, e.g.

import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Help()

The same help text is also found below.
In the below listing any function parameters surrounded by [ ] are optional and may be omitted.

Board Settings

These values should be accessed from a particular instance of the library, and may be set / read differently if more then one is created.
Set these after creating an instance but before calling Init(), for example to set the I²C address:

import UltraBorg
UB = UltraBorg.UltraBorg()
UB.i2cAddress = 0x10
UB.Init()

When reading these values to see if things have worked read after calling Init(), for example to see if the board was found:

import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Init()
if UB.foundChip:
    print 'Yay !'
else:
    print 'Aww...'

busNumber

I²C bus on which the UltraBorg is attached (Rev 1 is bus 0, Rev 2 is bus 1), only applies to the Raspberry Pi

bus

The smbus object used to talk to the I²C bus, only use this to talk with other I²C devices, only applies to the Raspberry Pi

i2cAddress

The I²C address of the UltraBorg chip to control

foundChip

True if the UltraBorg chip can be seen, False otherwise, on an Arduino check the return value from UbInit() instead

printFunction

Function reference to call when printing text, if None "print" is used, only applies to the Raspberry Pi

Board Functions

These represent the majority of the functionality of the UltraBorg, such as setting servo positions.
Call these functions on an instance, after calling Init(), e.g.

import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Init()
UB.SetServoPosition1(0.4)

Print(message)

Wrapper used by the UltraBorg instance to print messages, will call printFunction if set, print otherwise
Not applicable to Arduino

NoPrint(message)

Does nothing, intended for disabling diagnostic printout by using:
UB = UltraBorg.UltraBorg()
UB.printFunction = UB.NoPrint
Not applicable to Arduino

Init([tryOtherBus])

Prepare the I2C driver for talking to the UltraBorg
If tryOtherBus is True or omitted, this function will attempt to use the other bus if the UltraBorg devices can not be found on the current busNumber
tryOtherBus is not applicable to Arduino

value = GetWithRetry(function, count)

Attempts to read a value multiple times before giving up
Pass a get function with no parameters
e.g.
distance = GetWithRetry(UB.GetDistance1, 5)
Will try UB.GetDistance1() upto 5 times, returning when it gets a value
Useful for ensuring a read is successful
Not applicable to Arduino

worked = SetWithRetry(setFunction, getFunction, value, count)

Attempts to write a value multiple times before giving up
Pass a set function with one parameter, and a get function no parameters
The get function will be used to check if the set worked, if not it will be repeated
e.g.
worked = SetWithRetry(UB.SetServoMinimum1, UB.GetServoMinimum1, 2000, 5)
Will try UB.SetServoMinimum1(2000) upto 5 times, returning when UB.GetServoMinimum1 returns 2000.
Useful for ensuring a write is successful
Not applicable to Arduino

i2cRecv = ReadWithCheck(address, command, length)

Attempts an I2C read, checks if the bus tried to read too fast and re-tries
Only intended for internal usage
Not applicable to Arduino

distance = GetDistance1()

Gets the filtered distance for ultrasonic module #1 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance1 instead (no filtering)
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetDistance2()

Gets the filtered distance for ultrasonic module #2 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance2 instead (no filtering)
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetDistance3()

Gets the filtered distance for ultrasonic module #3 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance3 instead (no filtering)
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetDistance4()

Gets the filtered distance for ultrasonic module #4 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance4 instead (no filtering)
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetRawDistance1()

Gets the raw distance for ultrasonic module #1 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance1
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetRawDistance2()

Gets the raw distance for ultrasonic module #2 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance2
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetRawDistance3()

Gets the raw distance for ultrasonic module #3 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance3
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

distance = GetRawDistance4()

Gets the distance for ultrasonic module #4 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance4
e.g.
0 → No object in range
25 → Object 25 mm away
1000 → Object 1000 mm (1 m) away
3500 → Object 3500 mm (3.5 m) away

position = GetServoPosition1()

Gets the drive position for servo output #1
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

position = GetServoPosition2()

Gets the drive position for servo output #2
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

position = GetServoPosition3()

Gets the drive position for servo output #3
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

position = GetServoPosition4()

Gets the drive position for servo output #4
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

SetServoPosition1(position)

Sets the drive position for servo output #1
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

SetServoPosition2(position)

Sets the drive position for servo output #2
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

SetServoPosition3(position)

Sets the drive position for servo output #3
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

SetServoPosition4(position)

Sets the drive position for servo output #4
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0 → Central
0.5 → 50% to the right
1 → 100% to the right
-0.75 → 75% to the left

pwmLevel = GetServoMinimum1()

Gets the minimum PWM level for servo output #1
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMinimum2()

Gets the minimum PWM level for servo output #2
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMinimum3()

Gets the minimum PWM level for servo output #3
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMinimum4()

Gets the minimum PWM level for servo output #4
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMaximum1()

Gets the maximum PWM level for servo output #1
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMaximum2()

Gets the maximum PWM level for servo output #2
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMaximum3()

Gets the maximum PWM level for servo output #3
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoMaximum4()

Gets the maximum PWM level for servo output #4
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoStartup1()

Gets the startup PWM level for servo output #1
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoStartup2()

Gets the startup PWM level for servo output #2
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoStartup3()

Gets the startup PWM level for servo output #3
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre
5000 → 2.5 ms servo burst, higher than typical longest burst

pwmLevel = GetServoStartup4()

Gets the startup PWM level for servo output #4
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000 → 1 ms servo burst, typical shortest burst
4000 → 2 ms servo burst, typical longest burst
3000 → 1.5 ms servo burst, typical centre,
5000 → 2.5 ms servo burst, higher than typical longest burst

CalibrateServoPosition1(pwmLevel)

Sets the raw PWM level for servo output #1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

CalibrateServoPosition2(pwmLevel)

Sets the raw PWM level for servo output #2
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

CalibrateServoPosition3(pwmLevel)

Sets the raw PWM level for servo output #3
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

CalibrateServoPosition4(pwmLevel)

Sets the raw PWM level for servo output #4
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

pwmLevel = GetRawServoPosition1()

Gets the raw PWM level for servo output #1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition1
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

pwmLevel = GetRawServoPosition2()

Gets the raw PWM level for servo output #2
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition2
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

pwmLevel = GetRawServoPosition3()

Gets the raw PWM level for servo output #3
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition3
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

pwmLevel = GetRawServoPosition4()

Gets the raw PWM level for servo output #4
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition4
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMinimum1(pwmLevel)

Sets the minimum PWM level for servo output #1
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMinimum2(pwmLevel)

Sets the minimum PWM level for servo output #2
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMinimum3(pwmLevel)

Sets the minimum PWM level for servo output #3
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMinimum4(pwmLevel)

Sets the minimum PWM level for servo output #4
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMaximum1(pwmLevel)

Sets the maximum PWM level for servo output #1
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMaximum2(pwmLevel)

Sets the maximum PWM level for servo output #2
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMaximum3(pwmLevel)

Sets the maximum PWM level for servo output #3
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoMaximum4(pwmLevel)

Sets the maximum PWM level for servo output #4
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoStartup1(pwmLevel)

Sets the startup PWM level for servo output #1
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoStartup2(pwmLevel)

Sets the startup PWM level for servo output #2
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoStartup3(pwmLevel)

Sets the startup PWM level for servo output #3
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

SetServoStartup4(pwmLevel)

Sets the startup PWM level for servo output #4
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000 → 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000 → 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000 → 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000 → 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle

Help()

Displays the names and descriptions of the various functions and settings provided
Not applicable to Arduino

Code Listings

ubSequence.py

#!/usr/bin/env python
# coding: latin-1

# Import the libraries we need
import UltraBorg
import time

# Settings
servoMin = -1.0                 # Smallest servo position to use
servoMax = +1.0                 # Largest servo position to use
startupDelay = 0.5              # Delay before making the initial move
stepDelay = 0.1                 # Delay between steps
rateStart = 0.05                # Step distance for all servos during initial move
rateServo1 = 0.01               # Step distance for servo #1
rateServo2 = 0.02               # Step distance for servo #2
rateServo3 = 0.04               # Step distance for servo #3
rateServo4 = 0.08               # Step distance for servo #4

# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    print 'Move to central'
    # Initial settings
    servo1 = 0.0
    servo2 = 0.0
    servo3 = 0.0
    servo4 = 0.0
    # Set our initial servo positions
    UB.SetServoPosition1(servo1)
    UB.SetServoPosition2(servo2)
    UB.SetServoPosition3(servo3)
    UB.SetServoPosition4(servo4)
    # Wait a while to be sure the servos have caught up
    time.sleep(startupDelay)
    print 'Sweep to start position'
    while servo1 > servoMin:
        # Reduce the servo positions
        servo1 -= rateStart
        servo2 -= rateStart
        servo3 -= rateStart
        servo4 -= rateStart
        # Check if they are too small
        if servo1 < servoMin:
            servo1 = servoMin
            servo2 = servoMin
            servo3 = servoMin
            servo4 = servoMin
        # Set our new servo positions
        UB.SetServoPosition1(servo1)
        UB.SetServoPosition2(servo2)
        UB.SetServoPosition3(servo3)
        UB.SetServoPosition4(servo4)
        # Wait until the next step
        time.sleep(stepDelay)
    print 'Sweep all servos through the range'
    while True:
        # Increase the servo positions at separate rates
        servo1 += rateServo1
        servo2 += rateServo2
        servo3 += rateServo3
        servo4 += rateServo4
        # Check if any of them are too large, if so wrap to the over end
        if servo1 > servoMax:
            servo1 -= (servoMax - servoMin)
        if servo2 > servoMax:
            servo2 -= (servoMax - servoMin)
        if servo3 > servoMax:
            servo3 -= (servoMax - servoMin)
        if servo4 > servoMax:
            servo4 -= (servoMax - servoMin)
        # Set our new servo positions
        UB.SetServoPosition1(servo1)
        UB.SetServoPosition2(servo2)
        UB.SetServoPosition3(servo3)
        UB.SetServoPosition4(servo4)
        # Wait until the next step
        time.sleep(stepDelay)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    print 'Done'

ubReadDistances.py

#!/usr/bin/env python
# coding: latin-1

# Import the libraries we need
import UltraBorg
import time

# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    while True:
        # Read all four ultrasonic values
        usm1 = UB.GetDistance1()
        usm2 = UB.GetDistance2()
        usm3 = UB.GetDistance3()
        usm4 = UB.GetDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm2 = int(usm2)
        usm3 = int(usm3)
        usm4 = int(usm4)
        # Display the readings
        if usm1 == 0:
            print '#1 No reading'
        else:
            print '#1 % 4d mm' % (usm1)
        if usm2 == 0:
            print '#2 No reading'
        else:
            print '#2 % 4d mm' % (usm2)
        if usm3 == 0:
            print '#3 No reading'
        else:
            print '#3 % 4d mm' % (usm3)
        if usm4 == 0:
            print '#4 No reading'
        else:
            print '#4 % 4d mm' % (usm4)
        print
        # Wait between readings
        time.sleep(.1)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    print 'Done'

ubServoFromDistances.py

#!/usr/bin/env python
# coding: latin-1

# Import the libraries we need
import UltraBorg
import time

# Settings
distanceMin = 100.0             # Minimum distance in mm, corresponds to servo at -100%
distanceMax = 300.0             # Maximum distance in mm, corresponds to servo at +100%

# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Calculate our divisor
distanceDiv = (distanceMax - distanceMin) / 2.0

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    # Initial settings
    servo1 = 0.0
    servo2 = 0.0
    servo3 = 0.0
    servo4 = 0.0
    while True:
        # Read all four ultrasonic values, we use the raw values so we respond quickly
        usm1 = UB.GetRawDistance1()
        usm2 = UB.GetRawDistance2()
        usm3 = UB.GetRawDistance3()
        usm4 = UB.GetRawDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm2 = int(usm2)
        usm3 = int(usm3)
        usm4 = int(usm4)
        # Generate the servo positions based on the distance readings
        if usm1 != 0:
            servo1 = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if servo1 > 1.0:
                servo1 = 1.0
            elif servo1 < -1.0:
                servo1 = -1.0
        if usm2 != 0:
            servo2 = ((usm2 - distanceMin) / distanceDiv) - 1.0
            if servo2 > 1.0:
                servo2 = 1.0
            elif servo2 < -1.0:
                servo2 = -1.0
        if usm3 != 0:
            servo3 = ((usm3 - distanceMin) / distanceDiv) - 1.0
            if servo3 > 1.0:
                servo3 = 1.0
            elif servo3 < -1.0:
                servo3 = -1.0
        if usm4 != 0:
            servo4 = ((usm4 - distanceMin) / distanceDiv) - 1.0
            if servo4 > 1.0:
                servo4 = 1.0
            elif servo4 < -1.0:
                servo4 = -1.0
        # Display our readings
        print '%4d mm -> %.1f %%' % (usm1, servo1 * 100.0)
        print '%4d mm -> %.1f %%' % (usm2, servo2 * 100.0)
        print '%4d mm -> %.1f %%' % (usm3, servo3 * 100.0)
        print '%4d mm -> %.1f %%' % (usm4, servo4 * 100.0)
        print
        # Set our new servo positions
        UB.SetServoPosition1(servo1)
        UB.SetServoPosition2(servo2)
        UB.SetServoPosition3(servo3)
        UB.SetServoPosition4(servo4)
        # Wait between readings
        time.sleep(.1)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    print 'Done'

ubGui.py

#!/usr/bin/env python
# coding: latin-1

# Import library functions we need 
import UltraBorg
import Tkinter

# Start the UltraBorg
global UB
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Class representing the GUI dialog
class UltraBorg_tk(Tkinter.Tk):
    # Constructor (called when the object is first created)
    def __init__(self, parent):
        Tkinter.Tk.__init__(self, parent)
        self.parent = parent
        self.protocol("WM_DELETE_WINDOW", self.OnExit) # Call the OnExit function when user closes the dialog
        self.Initialise()

    # Initialise the dialog
    def Initialise(self):
        global UB
        self.title('UltraBorg Example GUI')

        # Setup a grid of 4 sliders which command each servo output, plus 4 readings for the servo positions and distances
        self.grid()

        # The servo sliders
        self.sld1 = Tkinter.Scale(self, from_ = +100, to = -100, orient = Tkinter.VERTICAL, command = self.sld1_move)
        self.sld1.set(0)
        self.sld1.grid(column = 0, row = 0, rowspan = 1, columnspan = 1, sticky = 'NSEW')
        self.sld2 = Tkinter.Scale(self, from_ = +100, to = -100, orient = Tkinter.VERTICAL, command = self.sld2_move)
        self.sld2.set(0)
        self.sld2.grid(column = 1, row = 0, rowspan = 1, columnspan = 1, sticky = 'NSEW')
        self.sld3 = Tkinter.Scale(self, from_ = +100, to = -100, orient = Tkinter.VERTICAL, command = self.sld3_move)
        self.sld3.set(0)
        self.sld3.grid(column = 2, row = 0, rowspan = 1, columnspan = 1, sticky = 'NSEW')
        self.sld4 = Tkinter.Scale(self, from_ = +100, to = -100, orient = Tkinter.VERTICAL, command = self.sld4_move)
        self.sld4.set(0)
        self.sld4.grid(column = 3, row = 0, rowspan = 1, columnspan = 1, sticky = 'NSEW')

        # The servo values (read from the controller)
        self.lblServo1 = Tkinter.Label(self, text = '-')
        self.lblServo1['font'] = ('Arial', 20, 'bold')
        self.lblServo1.grid(column = 0, row = 1, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblServo2 = Tkinter.Label(self, text = '-')
        self.lblServo2['font'] = ('Arial', 20, 'bold')
        self.lblServo2.grid(column = 1, row = 1, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblServo3 = Tkinter.Label(self, text = '-')
        self.lblServo3['font'] = ('Arial', 20, 'bold')
        self.lblServo3.grid(column = 2, row = 1, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblServo4 = Tkinter.Label(self, text = '-')
        self.lblServo4['font'] = ('Arial', 20, 'bold')
        self.lblServo4.grid(column = 3, row = 1, columnspan = 1, rowspan = 1, sticky = 'NSEW')

        # The distance readings and a heading
        self.lblDistanceHeading = Tkinter.Label(self, text = 'Distances (mm)')
        self.lblDistanceHeading['font'] = ('Arial', 20, 'bold')
        self.lblDistanceHeading.grid(column = 0, row = 2, columnspan = 4, rowspan = 1, sticky = 'NSEW')
        self.lblDistance1 = Tkinter.Label(self, text = '-')
        self.lblDistance1['font'] = ('Arial', 20, 'bold')
        self.lblDistance1.grid(column = 0, row = 3, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblDistance2 = Tkinter.Label(self, text = '-')
        self.lblDistance2['font'] = ('Arial', 20, 'bold')
        self.lblDistance2.grid(column = 1, row = 3, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblDistance3 = Tkinter.Label(self, text = '-')
        self.lblDistance3['font'] = ('Arial', 20, 'bold')
        self.lblDistance3.grid(column = 2, row = 3, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblDistance4 = Tkinter.Label(self, text = '-')
        self.lblDistance4['font'] = ('Arial', 20, 'bold')
        self.lblDistance4.grid(column = 3, row = 3, columnspan = 1, rowspan = 1, sticky = 'NSEW')

        # The grid sizing
        self.grid_columnconfigure(0, weight = 1)
        self.grid_columnconfigure(1, weight = 1)
        self.grid_columnconfigure(2, weight = 1)
        self.grid_columnconfigure(3, weight = 1)
        self.grid_rowconfigure(0, weight = 4)
        self.grid_rowconfigure(1, weight = 1)
        self.grid_rowconfigure(2, weight = 1)
        self.grid_rowconfigure(3, weight = 1)

        # Set the size of the dialog
        self.resizable(True, True)
        self.geometry('400x600')

        # Start polling for readings
        self.poll()

    # Polling function
    def poll(self):
        global UB

        # Read the servo positions
        servo1 = UB.GetServoPosition1()
        servo2 = UB.GetServoPosition2()
        servo3 = UB.GetServoPosition3()
        servo4 = UB.GetServoPosition4()

        # Read the ultrasonic distances
        distance1 = int(UB.GetDistance1())
        distance2 = int(UB.GetDistance2())
        distance3 = int(UB.GetDistance3())
        distance4 = int(UB.GetDistance4())

        # Set the servo displays
        self.lblServo1['text'] = '%.0f %%' % (servo1 * 100.0)
        self.lblServo2['text'] = '%.0f %%' % (servo2 * 100.0)
        self.lblServo3['text'] = '%.0f %%' % (servo3 * 100.0)
        self.lblServo4['text'] = '%.0f %%' % (servo4 * 100.0)

        # Set the ultrasonic displays
        if distance1 == 0:
            self.lblDistance1['text'] = 'None'
        else:
            self.lblDistance1['text'] = '%4d' % (distance1)
        if distance2 == 0:
            self.lblDistance2['text'] = 'None'
        else:
            self.lblDistance2['text'] = '%4d' % (distance2)
        if distance3 == 0:
            self.lblDistance3['text'] = 'None'
        else:
            self.lblDistance3['text'] = '%4d' % (distance3)
        if distance4 == 0:
            self.lblDistance4['text'] = 'None'
        else:
            self.lblDistance4['text'] = '%4d' % (distance4)

        # Prime the next poll
        self.after(200, self.poll)

    # Called when the user closes the dialog
    def OnExit(self):
        # End the program
        self.quit()

    # Called when sld1 is moved
    def sld1_move(self, value):
        global UB
        UB.SetServoPosition1(float(value) / 100.0)

    # Called when sld2 is moved
    def sld2_move(self, value):
        global UB
        UB.SetServoPosition2(float(value) / 100.0)

    # Called when sld3 is moved
    def sld3_move(self, value):
        global UB
        UB.SetServoPosition3(float(value) / 100.0)

    # Called when sld4 is moved
    def sld4_move(self, value):
        global UB
        UB.SetServoPosition4(float(value) / 100.0)

# if we are the main program (python was passed a script) load the dialog automatically
if __name__ == "__main__":
    app = UltraBorg_tk(None)
    app.mainloop()

UltraBorg.py

#!/usr/bin/env python
# coding: latin-1
"""
This module is designed to communicate with the UltraBorg

Use by creating an instance of the class, call the Init function, then command as desired, e.g.
import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Init()
# User code here, use UB to control the board

Multiple boards can be used when configured with different I²C addresses by creating multiple instances, e.g.
import UltraBorg
UB1 = UltraBorg.UltraBorg()
UB2 = UltraBorg.UltraBorg()
UB1.i2cAddress = 0x44
UB2.i2cAddress = 0x45
UB1.Init()
UB2.Init()
# User code here, use UB1 and UB2 to control each board separately

For explanations of the functions available call the Help function, e.g.
import UltraBorg
UB = UltraBorg.UltraBorg()
UB.Help()
See the website at www.piborg.org/ultraborg for more details
"""

# Import the libraries we need
import smbus
import types
import time

# Constant values
I2C_MAX_LEN             = 4
USM_US_TO_MM            = 0.171500
PWM_MIN                 = 2000  # Should be a 1 ms burst, typical servo minimum
PWM_MAX                 = 4000  # Should be a 2 ms burst, typical servo maximum
DELAY_AFTER_EEPROM      = 0.01  # Time to wait after updating an EEPROM value before reading
PWM_UNSET               = 0xFFFF
READ_TOO_FAST           = 0xED  # We seem to get this for the first byte sometimes on the RPi v2

I2C_ID_SERVO_USM        = 0x36

COMMAND_GET_TIME_USM1   = 1     # Get the time measured by ultrasonic #1 in us (0 for no detection)
COMMAND_GET_TIME_USM2   = 2     # Get the time measured by ultrasonic #2 in us (0 for no detection)
COMMAND_GET_TIME_USM3   = 3     # Get the time measured by ultrasonic #3 in us (0 for no detection)
COMMAND_GET_TIME_USM4   = 4     # Get the time measured by ultrasonic #4 in us (0 for no detection)
COMMAND_SET_PWM1        = 5     # Set the PWM duty cycle for drive #1 (16 bit)
COMMAND_GET_PWM1        = 6     # Get the PWM duty cycle for drive #1 (16 bit)
COMMAND_SET_PWM2        = 7     # Set the PWM duty cycle for drive #2 (16 bit)
COMMAND_GET_PWM2        = 8     # Get the PWM duty cycle for drive #2 (16 bit)
COMMAND_SET_PWM3        = 9     # Set the PWM duty cycle for drive #3 (16 bit)
COMMAND_GET_PWM3        = 10    # Get the PWM duty cycle for drive #3 (16 bit)
COMMAND_SET_PWM4        = 11    # Set the PWM duty cycle for drive #4 (16 bit)
COMMAND_GET_PWM4        = 12    # Get the PWM duty cycle for drive #4 (16 bit)
COMMAND_CALIBRATE_PWM1  = 13    # Set the PWM duty cycle for drive #1 (16 bit, ignores limit checks)
COMMAND_CALIBRATE_PWM2  = 14    # Set the PWM duty cycle for drive #2 (16 bit, ignores limit checks)
COMMAND_CALIBRATE_PWM3  = 15    # Set the PWM duty cycle for drive #3 (16 bit, ignores limit checks)
COMMAND_CALIBRATE_PWM4  = 16    # Set the PWM duty cycle for drive #4 (16 bit, ignores limit checks)
COMMAND_GET_PWM_MIN_1   = 17    # Get the minimum allowed PWM duty cycle for drive #1
COMMAND_GET_PWM_MAX_1   = 18    # Get the maximum allowed PWM duty cycle for drive #1
COMMAND_GET_PWM_BOOT_1  = 19    # Get the startup PWM duty cycle for drive #1
COMMAND_GET_PWM_MIN_2   = 20    # Get the minimum allowed PWM duty cycle for drive #2
COMMAND_GET_PWM_MAX_2   = 21    # Get the maximum allowed PWM duty cycle for drive #2
COMMAND_GET_PWM_BOOT_2  = 22    # Get the startup PWM duty cycle for drive #2
COMMAND_GET_PWM_MIN_3   = 23    # Get the minimum allowed PWM duty cycle for drive #3
COMMAND_GET_PWM_MAX_3   = 24    # Get the maximum allowed PWM duty cycle for drive #3
COMMAND_GET_PWM_BOOT_3  = 25    # Get the startup PWM duty cycle for drive #3
COMMAND_GET_PWM_MIN_4   = 26    # Get the minimum allowed PWM duty cycle for drive #4
COMMAND_GET_PWM_MAX_4   = 27    # Get the maximum allowed PWM duty cycle for drive #4
COMMAND_GET_PWM_BOOT_4  = 28    # Get the startup PWM duty cycle for drive #4
COMMAND_SET_PWM_MIN_1   = 29    # Set the minimum allowed PWM duty cycle for drive #1
COMMAND_SET_PWM_MAX_1   = 30    # Set the maximum allowed PWM duty cycle for drive #1
COMMAND_SET_PWM_BOOT_1  = 31    # Set the startup PWM duty cycle for drive #1
COMMAND_SET_PWM_MIN_2   = 32    # Set the minimum allowed PWM duty cycle for drive #2
COMMAND_SET_PWM_MAX_2   = 33    # Set the maximum allowed PWM duty cycle for drive #2
COMMAND_SET_PWM_BOOT_2  = 34    # Set the startup PWM duty cycle for drive #2
COMMAND_SET_PWM_MIN_3   = 35    # Set the minimum allowed PWM duty cycle for drive #3
COMMAND_SET_PWM_MAX_3   = 36    # Set the maximum allowed PWM duty cycle for drive #3
COMMAND_SET_PWM_BOOT_3  = 37    # Set the startup PWM duty cycle for drive #3
COMMAND_SET_PWM_MIN_4   = 38    # Set the minimum allowed PWM duty cycle for drive #4
COMMAND_SET_PWM_MAX_4   = 39    # Set the maximum allowed PWM duty cycle for drive #4
COMMAND_SET_PWM_BOOT_4  = 40    # Set the startup PWM duty cycle for drive #4
COMMAND_GET_FILTER_USM1 = 41    # Get the filtered time measured by ultrasonic #1 in us (0 for no detection)
COMMAND_GET_FILTER_USM2 = 42    # Get the filtered time measured by ultrasonic #2 in us (0 for no detection)
COMMAND_GET_FILTER_USM3 = 43    # Get the filtered time measured by ultrasonic #3 in us (0 for no detection)
COMMAND_GET_FILTER_USM4 = 44    # Get the filtered time measured by ultrasonic #4 in us (0 for no detection)
COMMAND_GET_ID          = 0x99  # Get the board identifier
COMMAND_SET_I2C_ADD     = 0xAA  # Set a new I2C address

COMMAND_VALUE_FWD       = 1     # I2C value representing forward
COMMAND_VALUE_REV       = 2     # I2C value representing reverse

COMMAND_VALUE_ON        = 1     # I2C value representing on
COMMAND_VALUE_OFF       = 0     # I2C value representing off


def ScanForUltraBorg(busNumber = 1):
    """
ScanForUltraBorg([busNumber])

Scans the I²C bus for a UltraBorg boards and returns a list of all usable addresses
The busNumber if supplied is which I²C bus to scan, 0 for Rev 1 boards, 1 for Rev 2 boards, if not supplied the default is 1
    """
    found = []
    print 'Scanning I²C bus #%d' % (busNumber)
    bus = smbus.SMBus(busNumber)
    for address in range(0x03, 0x78, 1):
        try:
            i2cRecv = bus.read_i2c_block_data(address, COMMAND_GET_ID, I2C_MAX_LEN)
            if len(i2cRecv) == I2C_MAX_LEN:
                if i2cRecv[1] == I2C_ID_SERVO_USM:
                    print 'Found UltraBorg at %02X' % (address)
                    found.append(address)
                else:
                    pass
            else:
                pass
        except KeyboardInterrupt:
            raise
        except:
            pass
    if len(found) == 0:
        print 'No UltraBorg boards found, is bus #%d correct (should be 0 for Rev 1, 1 for Rev 2)' % (busNumber)
    elif len(found) == 1:
        print '1 UltraBorg board found'
    else:
        print '%d UltraBorg boards found' % (len(found))
    return found


def SetNewAddress(newAddress, oldAddress = -1, busNumber = 1):
    """
SetNewAddress(newAddress, [oldAddress], [busNumber])

Scans the I²C bus for the first UltraBorg and sets it to a new I2C address
If oldAddress is supplied it will change the address of the board at that address rather than scanning the bus
The busNumber if supplied is which I²C bus to scan, 0 for Rev 1 boards, 1 for Rev 2 boards, if not supplied the default is 1
Warning, this new I²C address will still be used after resetting the power on the device
    """
    if newAddress < 0x03:
        print 'Error, I²C addresses below 3 (0x03) are reserved, use an address between 3 (0x03) and 119 (0x77)'
        return
    elif newAddress > 0x77:
        print 'Error, I²C addresses above 119 (0x77) are reserved, use an address between 3 (0x03) and 119 (0x77)'
        return
    if oldAddress < 0x0:
        found = ScanForUltraBorg(busNumber)
        if len(found) < 1:
            print 'No UltraBorg boards found, cannot set a new I²C address!'
            return
        else:
            oldAddress = found[0]
    print 'Changing I²C address from %02X to %02X (bus #%d)' % (oldAddress, newAddress, busNumber)
    bus = smbus.SMBus(busNumber)
    try:
        i2cRecv = bus.read_i2c_block_data(oldAddress, COMMAND_GET_ID, I2C_MAX_LEN)
        if len(i2cRecv) == I2C_MAX_LEN:
            if i2cRecv[1] == I2C_ID_SERVO_USM:
                foundChip = True
                print 'Found UltraBorg at %02X' % (oldAddress)
            else:
                foundChip = False
                print 'Found a device at %02X, but it is not a UltraBorg (ID %02X instead of %02X)' % (oldAddress, i2cRecv[1], I2C_ID_SERVO_USM)
        else:
            foundChip = False
            print 'Missing UltraBorg at %02X' % (oldAddress)
    except KeyboardInterrupt:
        raise
    except:
        foundChip = False
        print 'Missing UltraBorg at %02X' % (oldAddress)
    if foundChip:
        bus.write_byte_data(oldAddress, COMMAND_SET_I2C_ADD, newAddress)
        time.sleep(0.1)
        print 'Address changed to %02X, attempting to talk with the new address' % (newAddress)
        try:
            i2cRecv = bus.read_i2c_block_data(newAddress, COMMAND_GET_ID, I2C_MAX_LEN)
            if len(i2cRecv) == I2C_MAX_LEN:
                if i2cRecv[1] == I2C_ID_SERVO_USM:
                    foundChip = True
                    print 'Found UltraBorg at %02X' % (newAddress)
                else:
                    foundChip = False
                    print 'Found a device at %02X, but it is not a UltraBorg (ID %02X instead of %02X)' % (newAddress, i2cRecv[1], I2C_ID_SERVO_USM)
            else:
                foundChip = False
                print 'Missing UltraBorg at %02X' % (newAddress)
        except KeyboardInterrupt:
            raise
        except:
            foundChip = False
            print 'Missing UltraBorg at %02X' % (newAddress)
    if foundChip:
        print 'New I²C address of %02X set successfully' % (newAddress)
    else:
        print 'Failed to set new I²C address...'


# Class used to control UltraBorg
class UltraBorg:
    """
This module is designed to communicate with the UltraBorg

busNumber               I²C bus on which the UltraBorg is attached (Rev 1 is bus 0, Rev 2 is bus 1)
bus                     the smbus object used to talk to the I²C bus
i2cAddress              The I²C address of the UltraBorg chip to control
foundChip               True if the UltraBorg chip can be seen, False otherwise
printFunction           Function reference to call when printing text, if None "print" is used
    """

    # Shared values used by this class
    busNumber               = 1                 # Check here for Rev 1 vs Rev 2 and select the correct bus
    i2cAddress              = I2C_ID_SERVO_USM  # I²C address, override for a different address
    bus                     = None
    foundChip               = False
    printFunction           = None

    # Default calibration adjustments to standard values
    PWM_MIN_1               = PWM_MIN
    PWM_MAX_1               = PWM_MAX
    PWM_MIN_2               = PWM_MIN
    PWM_MAX_2               = PWM_MAX
    PWM_MIN_3               = PWM_MIN
    PWM_MAX_3               = PWM_MAX
    PWM_MIN_4               = PWM_MIN
    PWM_MAX_4               = PWM_MAX


    def Print(self, message):
        """
Print(message)

Wrapper used by the UltraBorg instance to print messages, will call printFunction if set, print otherwise
        """
        if self.printFunction == None:
            print message
        else:
            self.printFunction(message)


    def NoPrint(self, message):
        """
NoPrint(message)

Does nothing, intended for disabling diagnostic printout by using:
UB = UltraBorg.UltraBorg()
UB.printFunction = UB.NoPrint
        """
        pass


    def Init(self, tryOtherBus = True):
        """
Init([tryOtherBus])

Prepare the I2C driver for talking to the UltraBorg
If tryOtherBus is True or omitted, this function will attempt to use the other bus if the UltraBorg devices can not be found on the current busNumber
        """
        self.Print('Loading UltraBorg on bus %d, address %02X' % (self.busNumber, self.i2cAddress))

        # Open the bus
        self.bus = smbus.SMBus(self.busNumber)

        # Check for UltraBorg
        try:
            i2cRecv = self.bus.read_i2c_block_data(self.i2cAddress, COMMAND_GET_ID, I2C_MAX_LEN)
            if len(i2cRecv) == I2C_MAX_LEN:
                if i2cRecv[1] == I2C_ID_SERVO_USM:
                    self.foundChip = True
                    self.Print('Found UltraBorg at %02X' % (self.i2cAddress))
                else:
                    self.foundChip = False
                    self.Print('Found a device at %02X, but it is not a UltraBorg (ID %02X instead of %02X)' % (self.i2cAddress, i2cRecv[1], I2C_ID_SERVO_USM))
            else:
                self.foundChip = False
                self.Print('Missing UltraBorg at %02X' % (self.i2cAddress))
        except KeyboardInterrupt:
            raise
        except:
            self.foundChip = False
            self.Print('Missing UltraBorg at %02X' % (self.i2cAddress))

        # See if we are missing chips
        if not self.foundChip:
            self.Print('UltraBorg was not found')
            if tryOtherBus:
                if self.busNumber == 1:
                    self.busNumber = 0
                else:
                    self.busNumber = 1
                self.Print('Trying bus %d instead' % (self.busNumber))
                self.Init(False)
            else:
                self.Print('Are you sure your UltraBorg is properly attached, the correct address is used, and the I2C drivers are running?')
                self.bus = None
        else:
            self.Print('UltraBorg loaded on bus %d' % (self.busNumber))

        # Read the calibration settings from the UltraBorg
        self.PWM_MIN_1 = self.GetWithRetry(self.GetServoMinimum1, 5)
        self.PWM_MAX_1 = self.GetWithRetry(self.GetServoMaximum1, 5)
        self.PWM_MIN_2 = self.GetWithRetry(self.GetServoMinimum2, 5)
        self.PWM_MAX_2 = self.GetWithRetry(self.GetServoMaximum2, 5)
        self.PWM_MIN_3 = self.GetWithRetry(self.GetServoMinimum3, 5)
        self.PWM_MAX_3 = self.GetWithRetry(self.GetServoMaximum3, 5)
        self.PWM_MIN_4 = self.GetWithRetry(self.GetServoMinimum4, 5)
        self.PWM_MAX_4 = self.GetWithRetry(self.GetServoMaximum4, 5)


    def GetWithRetry(self, function, count):
        """
value = GetWithRetry(function, count)

Attempts to read a value multiple times before giving up
Pass a get function with no parameters
e.g.
distance = GetWithRetry(UB.GetDistance1, 5)
Will try UB.GetDistance1() upto 5 times, returning when it gets a value
Useful for ensuring a read is successful
        """
        value = None
        for i in range(count):
            okay = True
            try:
                value = function()
            except KeyboardInterrupt:
                raise
            except:
                okay = False
            if okay:
                break
        return value


    def SetWithRetry(self, setFunction, getFunction, value, count):
        """
worked = SetWithRetry(setFunction, getFunction, value, count)

Attempts to write a value multiple times before giving up
Pass a set function with one parameter, and a get function no parameters
The get function will be used to check if the set worked, if not it will be repeated
e.g.
worked = SetWithRetry(UB.SetServoMinimum1, UB.GetServoMinimum1, 2000, 5)
Will try UB.SetServoMinimum1(2000) upto 5 times, returning when UB.GetServoMinimum1 returns 2000.
Useful for ensuring a write is successful
        """
        for i in range(count):
            okay = True
            try:
                setFunction(value)
                readValue = getFunction()
            except KeyboardInterrupt:
                raise
            except:
                okay = False
            if okay:
                if readValue == value:
                    break
                else:
                    okay = False
        return okay


    def ReadWithCheck(self, address, command, length):
        """
i2cRecv = ReadWithCheck(address, command, length)

Attempts an I2C read, checks if the bus tried to read too fast and re-tries
Only intended for internal usage
        """
        unfinished = True
        while unfinished:
            i2cRecv = self.bus.read_i2c_block_data(address, command, length)
            if len(i2cRecv) > 0:
                if i2cRecv[0] == READ_TOO_FAST:
                    self.Print('I2C read was too fast, retrying')
                else:
                    unfinished = False
            else:
                self.Print('Empty I2C reply, retrying...')
        return i2cRecv


    def GetDistance1(self):
        """
distance = GetDistance1()

Gets the filtered distance for ultrasonic module #1 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance1 instead (no filtering)
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_FILTER_USM1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #1 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetDistance2(self):
        """
distance = GetDistance2()

Gets the filtered distance for ultrasonic module #2 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance2 instead (no filtering)
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_FILTER_USM2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #2 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetDistance3(self):
        """
distance = GetDistance3()

Gets the filtered distance for ultrasonic module #3 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance3 instead (no filtering)
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_FILTER_USM3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #3 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetDistance4(self):
        """
distance = GetDistance4()

Gets the filtered distance for ultrasonic module #4 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
If you need a faster response try GetRawDistance4 instead (no filtering)
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_FILTER_USM4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #4 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM



    def GetRawDistance1(self):
        """
distance = GetRawDistance1()

Gets the raw distance for ultrasonic module #1 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance1
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_TIME_USM1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #1 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetRawDistance2(self):
        """
distance = GetRawDistance2()

Gets the raw distance for ultrasonic module #2 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance2
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_TIME_USM2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #2 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetRawDistance3(self):
        """
distance = GetRawDistance3()

Gets the raw distance for ultrasonic module #3 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance3
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_TIME_USM3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #3 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetRawDistance4(self):
        """
distance = GetRawDistance4()

Gets the distance for ultrasonic module #4 in millimeters
Returns 0 for no object detected or no ultrasonic module attached
For a filtered (less jumpy) reading use GetDistance4
e.g.
0     -> No object in range
25    -> Object 25 mm away
1000  -> Object 1000 mm (1 m) away
3500  -> Object 3500 mm (3.5 m) away
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_TIME_USM4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading ultrasonic #4 distance!')
            return

        time_us = (i2cRecv[1] << 8) + i2cRecv[2]
        if time_us == 65535:
            time_us = 0
        return time_us * USM_US_TO_MM


    def GetServoPosition1(self):
        """
position = GetServoPosition1()

Gets the drive position for servo output #1
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo output #1!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        powerOut = (float(pwmDuty) - self.PWM_MIN_1) / (self.PWM_MAX_1 - self.PWM_MIN_1)
        return (2.0 * powerOut) - 1.0


    def GetServoPosition2(self):
        """
position = GetServoPosition2()

Gets the drive position for servo output #2
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo output #2!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        powerOut = (float(pwmDuty) - self.PWM_MIN_2) / (self.PWM_MAX_2 - self.PWM_MIN_2)
        return (2.0 * powerOut) - 1.0


    def GetServoPosition3(self):
        """
position = GetServoPosition3()

Gets the drive position for servo output #3
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo output #3!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        powerOut = (float(pwmDuty) - self.PWM_MIN_3) / (self.PWM_MAX_3 - self.PWM_MIN_3)
        return (2.0 * powerOut) - 1.0


    def GetServoPosition4(self):
        """
position = GetServoPosition4()

Gets the drive position for servo output #4
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo output #4!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        powerOut = (float(pwmDuty) - self.PWM_MIN_4) / (self.PWM_MAX_4 - self.PWM_MIN_4)
        return (2.0 * powerOut) - 1.0


    def SetServoPosition1(self, position):
        """
SetServoPosition1(position)

Sets the drive position for servo output #1
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        powerOut = (position + 1.0) / 2.0
        pwmDuty = int((powerOut * (self.PWM_MAX_1 - self.PWM_MIN_1)) + self.PWM_MIN_1)
        pwmDutyLow = pwmDuty & 0xFF
        pwmDutyHigh = (pwmDuty >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM1, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo output #1!')


    def SetServoPosition2(self, position):
        """
SetServoPosition2(position)

Sets the drive position for servo output #2
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        powerOut = (position + 1.0) / 2.0
        pwmDuty = int((powerOut * (self.PWM_MAX_2 - self.PWM_MIN_2)) + self.PWM_MIN_2)
        pwmDutyLow = pwmDuty & 0xFF
        pwmDutyHigh = (pwmDuty >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM2, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo output #2!')


    def SetServoPosition3(self, position):
        """
SetServoPosition3(position)

Sets the drive position for servo output #3
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        powerOut = (position + 1.0) / 2.0
        pwmDuty = int((powerOut * (self.PWM_MAX_3 - self.PWM_MIN_3)) + self.PWM_MIN_3)
        pwmDutyLow = pwmDuty & 0xFF
        pwmDutyHigh = (pwmDuty >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM3, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo output #3!')


    def SetServoPosition4(self, position):
        """
SetServoPosition4(position)

Sets the drive position for servo output #4
0 is central, -1 is maximum left, +1 is maximum right
e.g.
0     -> Central
0.5   -> 50% to the right
1     -> 100% to the right
-0.75 -> 75% to the left
        """
        powerOut = (position + 1.0) / 2.0
        pwmDuty = int((powerOut * (self.PWM_MAX_4 - self.PWM_MIN_4)) + self.PWM_MIN_4)
        pwmDutyLow = pwmDuty & 0xFF
        pwmDutyHigh = (pwmDuty >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM4, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo output #1!')


    def GetServoMinimum1(self):
        """
pwmLevel = GetServoMinimum1()

Gets the minimum PWM level for servo output #1
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MIN_1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #1 minimum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMinimum2(self):
        """
pwmLevel = GetServoMinimum2()

Gets the minimum PWM level for servo output #2
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MIN_2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #2 minimum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMinimum3(self):
        """
pwmLevel = GetServoMinimum3()

Gets the minimum PWM level for servo output #3
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MIN_3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #3 minimum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMinimum4(self):
        """
pwmLevel = GetServoMinimum4()

Gets the minimum PWM level for servo output #4
This corresponds to position -1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MIN_4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #4 minimum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMaximum1(self):
        """
pwmLevel = GetServoMaximum1()

Gets the maximum PWM level for servo output #1
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MAX_1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #1 maximum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMaximum2(self):
        """
pwmLevel = GetServoMaximum2()

Gets the maximum PWM level for servo output #2
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MAX_2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #2 maximum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMaximum3(self):
        """
pwmLevel = GetServoMaximum3()

Gets the maximum PWM level for servo output #3
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MAX_3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #3 maximum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoMaximum4(self):
        """
pwmLevel = GetServoMaximum4()

Gets the maximum PWM level for servo output #4
This corresponds to position +1
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_MAX_4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #4 maximum burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoStartup1(self):
        """
pwmLevel = GetServoStartup1()

Gets the startup PWM level for servo output #1
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_BOOT_1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #1 startup burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoStartup2(self):
        """
pwmLevel = GetServoStartup2()

Gets the startup PWM level for servo output #2
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_BOOT_2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #2 startup burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoStartup3(self):
        """
pwmLevel = GetServoStartup3()

Gets the startup PWM level for servo output #3
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_BOOT_3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #3 startup burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def GetServoStartup4(self):
        """
pwmLevel = GetServoStartup4()

Gets the startup PWM level for servo output #4
This can be anywhere in the minimum to maximum range
The value is an integer where 2000 represents a 1 ms servo burst
e.g.
2000  -> 1 ms servo burst, typical shortest burst
4000  -> 2 ms servo burst, typical longest burst
3000  -> 1.5 ms servo burst, typical centre, 
5000  -> 2.5 ms servo burst, higher than typical longest burst 
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM_BOOT_4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading servo #4 startup burst!')
            return

        return (i2cRecv[1] << 8) + i2cRecv[2]


    def CalibrateServoPosition1(self, pwmLevel):
        """
CalibrateServoPosition1(pwmLevel)

Sets the raw PWM level for servo output #1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_CALIBRATE_PWM1, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending calibration servo output #1!')


    def CalibrateServoPosition2(self, pwmLevel):
        """
CalibrateServoPosition2(pwmLevel)

Sets the raw PWM level for servo output #2
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_CALIBRATE_PWM2, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending calibration servo output #2!')


    def CalibrateServoPosition3(self, pwmLevel):
        """
CalibrateServoPosition3(pwmLevel)

Sets the raw PWM level for servo output #3
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_CALIBRATE_PWM3, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending calibration servo output #3!')


    def CalibrateServoPosition4(self, pwmLevel):
        """
CalibrateServoPosition4(pwmLevel)

Sets the raw PWM level for servo output #4
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_CALIBRATE_PWM4, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending calibration servo output #4!')


    def GetRawServoPosition1(self):
        """
pwmLevel = GetRawServoPosition1()

Gets the raw PWM level for servo output #1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition1
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM1, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading raw servo output #1!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        return pwmDuty


    def GetRawServoPosition2(self):
        """
pwmLevel = GetRawServoPosition2()

Gets the raw PWM level for servo output #2
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition2
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM2, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading raw servo output #2!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        return pwmDuty


    def GetRawServoPosition3(self):
        """
pwmLevel = GetRawServoPosition3()

Gets the raw PWM level for servo output #3
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition3
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM3, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading raw servo output #3!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        return pwmDuty


    def GetRawServoPosition4(self):
        """
pwmLevel = GetRawServoPosition4()

Gets the raw PWM level for servo output #4
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

This value requires interpreting into an actual servo position, this is already done by GetServoPosition4
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        try:
            i2cRecv = self.ReadWithCheck(self.i2cAddress, COMMAND_GET_PWM4, I2C_MAX_LEN)
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed reading raw servo output #4!')
            return

        pwmDuty = (i2cRecv[1] << 8) + i2cRecv[2]
        return pwmDuty


    def SetServoMinimum1(self, pwmLevel):
        """
SetServoMinimum1(pwmLevel)

Sets the minimum PWM level for servo output #1
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MIN_1, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo minimum limit #1!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MIN_1 = self.GetServoMinimum1()


    def SetServoMinimum2(self, pwmLevel):
        """
SetServoMinimum2(pwmLevel)

Sets the minimum PWM level for servo output #2
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MIN_2, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo minimum limit #2!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MIN_2 = self.GetServoMinimum2()


    def SetServoMinimum3(self, pwmLevel):
        """
SetServoMinimum3(pwmLevel)

Sets the minimum PWM level for servo output #3
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MIN_3, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo minimum limit #3!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MIN_3 = self.GetServoMinimum3()


    def SetServoMinimum4(self, pwmLevel):
        """
SetServoMinimum4(pwmLevel)

Sets the minimum PWM level for servo output #4
This corresponds to position -1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MIN_4, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo minimum limit #4!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MIN_4 = self.GetServoMinimum4()


    def SetServoMaximum1(self, pwmLevel):
        """
SetServoMaximum1(pwmLevel)

Sets the maximum PWM level for servo output #1
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MAX_1, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo maximum limit #1!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MAX_1 = self.GetServoMaximum1()


    def SetServoMaximum2(self, pwmLevel):
        """
SetServoMaximum2(pwmLevel)

Sets the maximum PWM level for servo output #2
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MAX_2, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo maximum limit #2!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MAX_2 = self.GetServoMaximum2()


    def SetServoMaximum3(self, pwmLevel):
        """
SetServoMaximum3(pwmLevel)

Sets the maximum PWM level for servo output #3
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MAX_3, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo maximum limit #3!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MAX_3 = self.GetServoMaximum3()


    def SetServoMaximum4(self, pwmLevel):
        """
SetServoMaximum4(pwmLevel)

Sets the maximum PWM level for servo output #4
This corresponds to position +1
This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle

Setting values outside the range of the servo for extended periods of time can damage the servo
LIMIT CHECKING IS ALTERED BY THIS COMMAND!
We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_MAX_4, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo maximum limit #4!')
        time.sleep(DELAY_AFTER_EEPROM)
        self.PWM_MAX_4 = self.GetServoMaximum4()


    def SetServoStartup1(self, pwmLevel):
        """
SetServoStartup1(pwmLevel)

Sets the startup PWM level for servo output #1
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition1 / GetServoPosition1
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF
        inRange = True

        if self.PWM_MIN_1 < self.PWM_MAX_1:
            # Normal direction
            if pwmLevel < self.PWM_MIN_1:
                inRange = False
            elif pwmLevel > self.PWM_MAX_1:
                inRange = False
        else:
            # Inverted direction
            if pwmLevel > self.PWM_MIN_1:
                inRange = False
            elif pwmLevel < self.PWM_MAX_1:
                inRange = False
        if pwmLevel == PWM_UNSET:
            # Force to unset behaviour (central)
            inRange = True

        if not inRange:
            print 'Servo #1 startup position %d is outside the limits of %d to %d' % (pwmLevel, self.PWM_MIN_1, self.PWM_MAX_1)
            return

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_BOOT_1, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo startup position #1!')
        time.sleep(DELAY_AFTER_EEPROM)


    def SetServoStartup2(self, pwmLevel):
        """
SetServoStartup2(pwmLevel)

Sets the startup PWM level for servo output #2
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition2 / GetServoPosition2
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF
        inRange = True

        if self.PWM_MIN_2 < self.PWM_MAX_2:
            # Normal direction
            if pwmLevel < self.PWM_MIN_2:
                inRange = False
            elif pwmLevel > self.PWM_MAX_2:
                inRange = False
        else:
            # Inverted direction
            if pwmLevel > self.PWM_MIN_2:
                inRange = False
            elif pwmLevel < self.PWM_MAX_2:
                inRange = False
        if pwmLevel == PWM_UNSET:
            # Force to unset behaviour (central)
            inRange = True

        if not inRange:
            print 'Servo #2 startup position %d is outside the limits of %d to %d' % (pwmLevel, self.PWM_MIN_2, self.PWM_MAX_2)
            return

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_BOOT_2, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo startup position #2!')
        time.sleep(DELAY_AFTER_EEPROM)


    def SetServoStartup3(self, pwmLevel):
        """
SetServoStartup3(pwmLevel)

Sets the startup PWM level for servo output #3
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition3 / GetServoPosition3
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF
        inRange = True

        if self.PWM_MIN_3 < self.PWM_MAX_3:
            # Normal direction
            if pwmLevel < self.PWM_MIN_3:
                inRange = False
            elif pwmLevel > self.PWM_MAX_3:
                inRange = False
        else:
            # Inverted direction
            if pwmLevel > self.PWM_MIN_3:
                inRange = False
            elif pwmLevel < self.PWM_MAX_3:
                inRange = False
        if pwmLevel == PWM_UNSET:
            # Force to unset behaviour (central)
            inRange = True

        if not inRange:
            print 'Servo #3 startup position %d is outside the limits of %d to %d' % (pwmLevel, self.PWM_MIN_3, self.PWM_MAX_3)
            return

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_BOOT_3, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo startup position #3!')
        time.sleep(DELAY_AFTER_EEPROM)


    def SetServoStartup4(self, pwmLevel):
        """
SetServoStartup4(pwmLevel)

Sets the startup PWM level for servo output #4
This can be anywhere in the minimum to maximum range

We recommend using the tuning GUI for setting the servo limits for SetServoPosition4 / GetServoPosition4
This value is checked against the current servo limits before setting

The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
e.g.
2000  -> 1 ms servo burst, typical shortest burst, ~3% duty cycle
4000  -> 2 ms servo burst, typical longest burst, ~ 6.1% duty cycle
3000  -> 1.5 ms servo burst, typical centre, ~4.6% duty cycle
5000  -> 2.5 ms servo burst, higher than typical longest burst, ~ 7.6% duty cycle
        """
        pwmDutyLow = pwmLevel & 0xFF
        pwmDutyHigh = (pwmLevel >> 8) & 0xFF
        inRange = True

        if self.PWM_MIN_4 < self.PWM_MAX_4:
            # Normal direction
            if pwmLevel < self.PWM_MIN_4:
                inRange = False
            elif pwmLevel > self.PWM_MAX_4:
                inRange = False
        else:
            # Inverted direction
            if pwmLevel > self.PWM_MIN_4:
                inRange = False
            elif pwmLevel < self.PWM_MAX_4:
                inRange = False
        if pwmLevel == PWM_UNSET:
            # Force to unset behaviour (central)
            inRange = True

        if not inRange:
            print 'Servo #4 startup position %d is outside the limits of %d to %d' % (pwmLevel, self.PWM_MIN_4, self.PWM_MAX_4)
            return

        try:
            self.bus.write_i2c_block_data(self.i2cAddress, COMMAND_SET_PWM_BOOT_4, [pwmDutyHigh, pwmDutyLow])
        except KeyboardInterrupt:
            raise
        except:
            self.Print('Failed sending servo startup position #4!')
        time.sleep(DELAY_AFTER_EEPROM)


    def Help(self):
        """
Help()

Displays the names and descriptions of the various functions and settings provided
        """
        funcList = [UltraBorg.__dict__.get(a) for a in dir(UltraBorg) if isinstance(UltraBorg.__dict__.get(a), types.FunctionType)]
        funcListSorted = sorted(funcList, key = lambda x: x.func_code.co_firstlineno)

        print self.__doc__
        print
        for func in funcListSorted:
            print '=== %s === %s' % (func.func_name, func.func_doc)

ubTuningGui.py

#!/usr/bin/env python
# coding: latin-1

# Import library functions we need 
import UltraBorg
import Tkinter
import Tix

# Start the UltraBorg
global UB
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Calibration settings
CAL_PWM_MIN = 0                 # Minimum selectable calibration burst (2000 = 1 ms)
CAL_PWM_MAX = 6000              # Maximum selectable calibration burst (2000 = 1 ms)
CAL_PWM_START = 3000            # Startup value for the calibration burst (2000 = 1 ms)
STD_PWM_MIN = 2000              # Default minimum position
STD_PWM_MAX = 4000              # Default maximum position
STD_PWM_START = 0xFFFF          # Default startup position (unset, calculates centre)

# Class representing the GUI dialog
class UltraBorg_tk(Tkinter.Tk):
    # Constructor (called when the object is first created)
    def __init__(self, parent):
        Tkinter.Tk.__init__(self, parent)
        self.tk.call('package', 'require', 'Tix')
        self.parent = parent
        self.protocol("WM_DELETE_WINDOW", self.OnExit) # Call the OnExit function when user closes the dialog
        self.Initialise()

    # Initialise the dialog
    def Initialise(self):
        global UB
        self.title('UltraBorg Tuning GUI')

        # Setup a grid of 4 sliders which command each servo output, plus 4 readings for the servo positions and distances
        self.grid()

        # The heading labels
        self.lblHeadingTask = Tkinter.Label(self, text = 'Task to perform')
        self.lblHeadingTask['font'] = ('Arial', 18, 'bold')
        self.lblHeadingTask.grid(column = 0, row = 0, columnspan = 1, rowspan = 1, sticky = 'NSEW')
        self.lblHeadingServo1 = Tkinter.Label(self, text = 'Servo #1')
        self.lblHeadingServo1['font'] = ('Arial', 18, 'bold')
        self.lblHeadingServo1.grid(column = 1, row = 0, columnspan = 2, rowspan = 1, sticky = 'NSEW')
        self.lblHeadingServo2 = Tkinter.Label(self, text = 'Servo #2')
        self.lblHeadingServo2['font'] = ('Arial', 18, 'bold')
        self.lblHeadingServo2.grid(column = 3, row = 0, columnspan = 2, rowspan = 1, sticky = 'NSEW')
        self.lblHeadingServo3 = Tkinter.Label(self, text = 'Servo #3')
        self.lblHeadingServo3['font'] = ('Arial', 18, 'bold')
        self.lblHeadingServo3.grid(column = 5, row = 0, columnspan = 2, rowspan = 1, sticky = 'NSEW')
        self.lblHeadingServo4 = Tkinter.Label(self, text = 'Servo #4')
        self.lblHeadingServo4['font'] = ('Arial', 18, 'bold')
        self.lblHeadingServo4.grid(column = 7, row = 0, columnspan = 2, rowspan = 1, sticky = 'NSEW')

        # The task descriptions
        self.lblTaskMaximum = Tkinter.Label(self, text = 
                'Hover over the buttons\n' + 
                'for more help\n\n' +
                'Set the servo maximums')
        self.lblTaskMaximum['font'] = ('Arial', 14, '')
        self.lblTaskMaximum.grid(column = 0, row = 1, columnspan = 1, rowspan = 2, sticky = 'NEW')
        self.lblTaskStartup = Tkinter.Label(self, text = 'Set the servo startup positions')
        self.lblTaskStartup['font'] = ('Arial', 14, '')
        self.lblTaskStartup.grid(column = 0, row = 3, columnspan = 1, rowspan = 2, sticky = 'NSEW')
        self.lblTaskMinimum = Tkinter.Label(self, text = 'Set the servo minimums')
        self.lblTaskMinimum['font'] = ('Arial', 14, '')
        self.lblTaskMinimum.grid(column = 0, row = 5, columnspan = 1, rowspan = 2, sticky = 'NSEW')
        self.lblTaskCurrent = Tkinter.Label(self, text = 'Current servo position')
        self.lblTaskCurrent['font'] = ('Arial', 18, 'bold')
        self.lblTaskCurrent.grid(column = 0, row = 7, columnspan = 1, rowspan = 1, sticky = 'NSEW')

        # The servo sliders
        self.sld1 = Tkinter.Scale(self, from_ = CAL_PWM_MAX, to = CAL_PWM_MIN, orient = Tkinter.VERTICAL, command = self.sld1_move, showvalue = 0)
        self.sld1.set(CAL_PWM_START)
        self.sld1.grid(column = 1, row = 1, rowspan = 6, columnspan = 1, sticky = 'NSE')
        self.sld2 = Tkinter.Scale(self, from_ = CAL_PWM_MAX, to = CAL_PWM_MIN, orient = Tkinter.VERTICAL, command = self.sld2_move, showvalue = 0)
        self.sld2.set(CAL_PWM_START)
        self.sld2.grid(column = 3, row = 1, rowspan = 6, columnspan = 1, sticky = 'NSE')
        self.sld3 = Tkinter.Scale(self, from_ = CAL_PWM_MAX, to = CAL_PWM_MIN, orient = Tkinter.VERTICAL, command = self.sld3_move, showvalue = 0)
        self.sld3.set(CAL_PWM_START)
        self.sld3.grid(column = 5, row = 1, rowspan = 6, columnspan = 1, sticky = 'NSE')
        self.sld4 = Tkinter.Scale(self, from_ = CAL_PWM_MAX, to = CAL_PWM_MIN, orient = Tkinter.VERTICAL, command = self.sld4_move, showvalue = 0)
        self.sld4.set(CAL_PWM_START)
        self.sld4.grid(column = 7, row = 1, rowspan = 6, columnspan = 1, sticky = 'NSE')

        # The servo maximums
        self.lblServoMaximum1 = Tkinter.Label(self, text = '-')
        self.lblServoMaximum1['font'] = ('Arial', 14, '')
        self.lblServoMaximum1.grid(column = 2, row = 1, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMaximum2 = Tkinter.Label(self, text = '-')
        self.lblServoMaximum2['font'] = ('Arial', 14, '')
        self.lblServoMaximum2.grid(column = 4, row = 1, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMaximum3 = Tkinter.Label(self, text = '-')
        self.lblServoMaximum3['font'] = ('Arial', 14, '')
        self.lblServoMaximum3.grid(column = 6, row = 1, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMaximum4 = Tkinter.Label(self, text = '-')
        self.lblServoMaximum4['font'] = ('Arial', 14, '')
        self.lblServoMaximum4.grid(column = 8, row = 1, columnspan = 1, rowspan = 1, sticky = 'SW')

        # The servo maximum set buttons
        self.butServoMaximum1 = Tkinter.Button(self, text = 'Save\nmaximum', command = self.butServoMaximum1_click)
        self.butServoMaximum1['font'] = ('Arial', 12, '')
        self.butServoMaximum1.grid(column = 2, row = 2, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMaximum2 = Tkinter.Button(self, text = 'Save\nmaximum', command = self.butServoMaximum2_click)
        self.butServoMaximum2['font'] = ('Arial', 12, '')
        self.butServoMaximum2.grid(column = 4, row = 2, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMaximum3 = Tkinter.Button(self, text = 'Save\nmaximum', command = self.butServoMaximum3_click)
        self.butServoMaximum3['font'] = ('Arial', 12, '')
        self.butServoMaximum3.grid(column = 6, row = 2, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMaximum4 = Tkinter.Button(self, text = 'Save\nmaximum', command = self.butServoMaximum4_click)
        self.butServoMaximum4['font'] = ('Arial', 12, '')
        self.butServoMaximum4.grid(column = 8, row = 2, columnspan = 1, rowspan = 1, sticky = 'NW')

        # The servo startups
        self.lblServoStartup1 = Tkinter.Label(self, text = '-')
        self.lblServoStartup1['font'] = ('Arial', 14, '')
        self.lblServoStartup1.grid(column = 2, row = 3, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoStartup2 = Tkinter.Label(self, text = '-')
        self.lblServoStartup2['font'] = ('Arial', 14, '')
        self.lblServoStartup2.grid(column = 4, row = 3, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoStartup3 = Tkinter.Label(self, text = '-')
        self.lblServoStartup3['font'] = ('Arial', 14, '')
        self.lblServoStartup3.grid(column = 6, row = 3, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoStartup4 = Tkinter.Label(self, text = '-')
        self.lblServoStartup4['font'] = ('Arial', 14, '')
        self.lblServoStartup4.grid(column = 8, row = 3, columnspan = 1, rowspan = 1, sticky = 'SW')

        # The servo startup set buttons
        self.butServoStartup1 = Tkinter.Button(self, text = 'Save\nstartup', command = self.butServoStartup1_click)
        self.butServoStartup1['font'] = ('Arial', 12, '')
        self.butServoStartup1.grid(column = 2, row = 4, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoStartup2 = Tkinter.Button(self, text = 'Save\nstartup', command = self.butServoStartup2_click)
        self.butServoStartup2['font'] = ('Arial', 12, '')
        self.butServoStartup2.grid(column = 4, row = 4, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoStartup3 = Tkinter.Button(self, text = 'Save\nstartup', command = self.butServoStartup3_click)
        self.butServoStartup3['font'] = ('Arial', 12, '')
        self.butServoStartup3.grid(column = 6, row = 4, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoStartup4 = Tkinter.Button(self, text = 'Save\nstartup', command = self.butServoStartup4_click)
        self.butServoStartup4['font'] = ('Arial', 12, '')
        self.butServoStartup4.grid(column = 8, row = 4, columnspan = 1, rowspan = 1, sticky = 'NW')

        # The servo minimums
        self.lblServoMinimum1 = Tkinter.Label(self, text = '-')
        self.lblServoMinimum1['font'] = ('Arial', 14, '')
        self.lblServoMinimum1.grid(column = 2, row = 5, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMinimum2 = Tkinter.Label(self, text = '-')
        self.lblServoMinimum2['font'] = ('Arial', 14, '')
        self.lblServoMinimum2.grid(column = 4, row = 5, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMinimum3 = Tkinter.Label(self, text = '-')
        self.lblServoMinimum3['font'] = ('Arial', 14, '')
        self.lblServoMinimum3.grid(column = 6, row = 5, columnspan = 1, rowspan = 1, sticky = 'SW')
        self.lblServoMinimum4 = Tkinter.Label(self, text = '-')
        self.lblServoMinimum4['font'] = ('Arial', 14, '')
        self.lblServoMinimum4.grid(column = 8, row = 5, columnspan = 1, rowspan = 1, sticky = 'SW')

        # The servo minimum set buttons
        self.butServoMinimum1 = Tkinter.Button(self, text = 'Save\nminimum', command = self.butServoMinimum1_click)
        self.butServoMinimum1['font'] = ('Arial', 12, '')
        self.butServoMinimum1.grid(column = 2, row = 6, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMinimum2 = Tkinter.Button(self, text = 'Save\nminimum', command = self.butServoMinimum2_click)
        self.butServoMinimum2['font'] = ('Arial', 12, '')
        self.butServoMinimum2.grid(column = 4, row = 6, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMinimum3 = Tkinter.Button(self, text = 'Save\nminimum', command = self.butServoMinimum3_click)
        self.butServoMinimum3['font'] = ('Arial', 12, '')
        self.butServoMinimum3.grid(column = 6, row = 6, columnspan = 1, rowspan = 1, sticky = 'NW')
        self.butServoMinimum4 = Tkinter.Button(self, text = 'Save\nminimum', command = self.butServoMinimum4_click)
        self.butServoMinimum4['font'] = ('Arial', 12, '')
        self.butServoMinimum4.grid(column = 8, row = 6, columnspan = 1, rowspan = 1, sticky = 'NW')

        # The servo values (read from the controller)
        self.lblServo1 = Tkinter.Label(self, text = '-')
        self.lblServo1['font'] = ('Arial', 18, '')
        self.lblServo1.grid(column = 2, row = 7, columnspan = 1, rowspan = 1, sticky = 'NSW')
        self.lblServo2 = Tkinter.Label(self, text = '-')
        self.lblServo2['font'] = ('Arial', 18, '')
        self.lblServo2.grid(column = 4, row = 7, columnspan = 1, rowspan = 1, sticky = 'NSW')
        self.lblServo3 = Tkinter.Label(self, text = '-')
        self.lblServo3['font'] = ('Arial', 18, '')
        self.lblServo3.grid(column = 6, row = 7, columnspan = 1, rowspan = 1, sticky = 'NSW')
        self.lblServo4 = Tkinter.Label(self, text = '-')
        self.lblServo4['font'] = ('Arial', 18, '')
        self.lblServo4.grid(column = 8, row = 7, columnspan = 1, rowspan = 1, sticky = 'NSW')

        # The major operations
        self.butReset = Tkinter.Button(self, text = 'Reset and save all to default values', command = self.butReset_click)
        self.butReset['font'] = ("Arial", 20, "bold")
        self.butReset.grid(column = 0, row = 8, rowspan = 1, columnspan = 9, sticky = 'NSEW')

        # Balloon help pop-up
        self.tipStatus = Tix.Balloon(self)
        self.servoSliderHelp = ('Use this slider to move servo #%d.\n' +
                                'Hover over each button for more help.\n' +
                                'The current position of servo #%d is shown at the bottom.')
        self.servoMaxHelp = ('Set the maximum for servo #%d.\n' + 
                             'Slowly move the servo #%d slider up until the servo stops moving,\n' + 
                             'then move the slider back down slightly to where it moves again.\n' +
                             'This will become +100%%.')
        self.servoStartupHelp = ('Set the startup position for servo #%d.\n' +
                                 'When UltraBorg powers up, servo #%d will move to this position.\n' + 
                                 'This position must be between the set maximum and minimum.\n' +
                                 'If unset then 0%% is used instead.')
        self.servoMinHelp = ('Set the minimum for servo #%d.\n' + 
                             'Slowly move the servo #%d slider down until the servo stops moving,\n' + 
                             'then move the slider back up slightly to where it moves again.\n' +
                             'This will become -100%%.')
        self.tipStatus.bind_widget(self.sld1,             balloonmsg = self.servoSliderHelp  % (1, 1))
        self.tipStatus.bind_widget(self.butServoMaximum1, balloonmsg = self.servoMaxHelp     % (1, 1))
        self.tipStatus.bind_widget(self.butServoStartup1, balloonmsg = self.servoStartupHelp % (1, 1))
        self.tipStatus.bind_widget(self.butServoMinimum1, balloonmsg = self.servoMinHelp     % (1, 1))
        self.tipStatus.bind_widget(self.sld2,             balloonmsg = self.servoSliderHelp  % (2, 2))
        self.tipStatus.bind_widget(self.butServoMaximum2, balloonmsg = self.servoMaxHelp     % (2, 2))
        self.tipStatus.bind_widget(self.butServoStartup2, balloonmsg = self.servoStartupHelp % (2, 2))
        self.tipStatus.bind_widget(self.butServoMinimum2, balloonmsg = self.servoMinHelp     % (2, 2))
        self.tipStatus.bind_widget(self.sld3,             balloonmsg = self.servoSliderHelp  % (3, 3))
        self.tipStatus.bind_widget(self.butServoMaximum3, balloonmsg = self.servoMaxHelp     % (3, 3))
        self.tipStatus.bind_widget(self.butServoStartup3, balloonmsg = self.servoStartupHelp % (3, 3))
        self.tipStatus.bind_widget(self.butServoMinimum3, balloonmsg = self.servoMinHelp     % (3, 3))
        self.tipStatus.bind_widget(self.sld4,             balloonmsg = self.servoSliderHelp  % (4, 4))
        self.tipStatus.bind_widget(self.butServoMaximum4, balloonmsg = self.servoMaxHelp     % (4, 4))
        self.tipStatus.bind_widget(self.butServoStartup4, balloonmsg = self.servoStartupHelp % (4, 4))
        self.tipStatus.bind_widget(self.butServoMinimum4, balloonmsg = self.servoMinHelp     % (4, 4))


        # The grid sizing
        self.grid_columnconfigure(0, weight = 1)
        self.grid_columnconfigure(1, weight = 1)
        self.grid_columnconfigure(2, weight = 2)
        self.grid_columnconfigure(3, weight = 1)
        self.grid_columnconfigure(4, weight = 2)
        self.grid_columnconfigure(5, weight = 1)
        self.grid_columnconfigure(6, weight = 2)
        self.grid_columnconfigure(7, weight = 1)
        self.grid_columnconfigure(8, weight = 2)
        self.grid_rowconfigure(0, weight = 1)
        self.grid_rowconfigure(1, weight = 1)
        self.grid_rowconfigure(2, weight = 1)
        self.grid_rowconfigure(3, weight = 1)
        self.grid_rowconfigure(4, weight = 1)
        self.grid_rowconfigure(5, weight = 1)
        self.grid_rowconfigure(6, weight = 1)
        self.grid_rowconfigure(7, weight = 1)
        self.grid_rowconfigure(8, weight = 1)

        # Set the size of the dialog
        self.resizable(True, True)
        self.geometry('1000x700')

        # Read the current settings for each servo
        self.ReadAllCalibration()

        # Start polling for readings
        self.poll()

    # Polling function
    def poll(self):
        global UB

        # Read the servo positions
        servo1 = UB.GetRawServoPosition1()
        servo2 = UB.GetRawServoPosition2()
        servo3 = UB.GetRawServoPosition3()
        servo4 = UB.GetRawServoPosition4()

        # Set the servo displays
        self.lblServo1['text'] = '%d' % (servo1)
        self.lblServo2['text'] = '%d' % (servo2)
        self.lblServo3['text'] = '%d' % (servo3)
        self.lblServo4['text'] = '%d' % (servo4)

        # Prime the next poll
        self.after(200, self.poll)

    # Reads all of the current calibration settings
    def ReadAllCalibration(self):
        self.SetLabelValue(self.lblServoMaximum1, UB.PWM_MAX_1)
        self.SetLabelValue(self.lblServoMaximum2, UB.PWM_MAX_2)
        self.SetLabelValue(self.lblServoMaximum3, UB.PWM_MAX_3)
        self.SetLabelValue(self.lblServoMaximum4, UB.PWM_MAX_4)
        self.SetLabelValue(self.lblServoMinimum1, UB.PWM_MIN_1)
        self.SetLabelValue(self.lblServoMinimum2, UB.PWM_MIN_2)
        self.SetLabelValue(self.lblServoMinimum3, UB.PWM_MIN_3)
        self.SetLabelValue(self.lblServoMinimum4, UB.PWM_MIN_4)
        self.SetLabelValue(self.lblServoStartup1, UB.GetWithRetry(UB.GetServoStartup1, 5))
        self.SetLabelValue(self.lblServoStartup2, UB.GetWithRetry(UB.GetServoStartup2, 5))
        self.SetLabelValue(self.lblServoStartup3, UB.GetWithRetry(UB.GetServoStartup3, 5))
        self.SetLabelValue(self.lblServoStartup4, UB.GetWithRetry(UB.GetServoStartup4, 5))

    # Takes a label and PWM drive level for display
    def SetLabelValue(self, label, pwmLevel):
        if pwmLevel == None:
            label['text'] = 'Unset'
        elif pwmLevel == 0x0000:
            label['text'] = 'Unset'
        elif pwmLevel == 0xFFFF:
            label['text'] = 'Unset'
        else:
            label['text'] = '%d' % (pwmLevel)

    # Takes a label and returns a PWM drive level or 0
    def GetLabelValue(self, label):
        try:
            return int(label['text'])
        except:
            return 0

    # Called when the user closes the dialog
    def OnExit(self):
        # End the program
        self.quit()

    # Called when sld1 is moved
    def sld1_move(self, value):
        global UB
        UB.CalibrateServoPosition1(int(value))

    # Called when sld2 is moved
    def sld2_move(self, value):
        global UB
        UB.CalibrateServoPosition2(int(value))

    # Called when sld3 is moved
    def sld3_move(self, value):
        global UB
        UB.CalibrateServoPosition3(int(value))

    # Called when sld4 is moved
    def sld4_move(self, value):
        global UB
        UB.CalibrateServoPosition4(int(value))

    # Called when butReset is clicked
    def butReset_click(self):
        global UB
        # Set all values back to standard
        UB.SetWithRetry(UB.SetServoMaximum1, UB.GetServoMaximum1, STD_PWM_MAX, 5)
        UB.SetWithRetry(UB.SetServoMinimum1, UB.GetServoMinimum1, STD_PWM_MIN, 5)
        UB.SetWithRetry(UB.SetServoStartup1, UB.GetServoStartup1, STD_PWM_START, 5)
        UB.SetWithRetry(UB.SetServoMaximum2, UB.GetServoMaximum2, STD_PWM_MAX, 5)
        UB.SetWithRetry(UB.SetServoMinimum2, UB.GetServoMinimum2, STD_PWM_MIN, 5)
        UB.SetWithRetry(UB.SetServoStartup2, UB.GetServoStartup2, STD_PWM_START, 5)
        UB.SetWithRetry(UB.SetServoMaximum3, UB.GetServoMaximum3, STD_PWM_MAX, 5)
        UB.SetWithRetry(UB.SetServoMinimum3, UB.GetServoMinimum3, STD_PWM_MIN, 5)
        UB.SetWithRetry(UB.SetServoStartup3, UB.GetServoStartup3, STD_PWM_START, 5)
        UB.SetWithRetry(UB.SetServoMaximum4, UB.GetServoMaximum4, STD_PWM_MAX, 5)
        UB.SetWithRetry(UB.SetServoMinimum4, UB.GetServoMinimum4, STD_PWM_MIN, 5)
        UB.SetWithRetry(UB.SetServoStartup4, UB.GetServoStartup4, STD_PWM_START, 5)
        # Move back to centre
        self.sld1.set(CAL_PWM_START)
        self.sld2.set(CAL_PWM_START)
        self.sld3.set(CAL_PWM_START)
        self.sld4.set(CAL_PWM_START)
        # Re-read calibration settings
        self.ReadAllCalibration()

    # Called when butServoMaximum1 is clicked
    def butServoMaximum1_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo1)
        if pwmLevel == 0:
            self.lblServoMaximum1['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMaximum1['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMaximum1, UB.GetServoMaximum1, pwmLevel, 5)
            if okay:
                self.lblServoMaximum1['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMaximum1['fg'] = '#000000'
            else:
                self.lblServoMaximum1['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMaximum1['fg'] = '#A00000'

    # Called when butServoMinimum1 is clicked
    def butServoMinimum1_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo1)
        if pwmLevel == 0:
            self.lblServoMinimum1['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMinimum1['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMinimum1, UB.GetServoMinimum1, pwmLevel, 5)
            if okay:
                self.lblServoMinimum1['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMinimum1['fg'] = '#000000'
            else:
                self.lblServoMinimum1['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMinimum1['fg'] = '#A00000'

    # Called when butServoStartup1 is clicked
    def butServoStartup1_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo1)
        if pwmLevel == 0:
            self.lblServoStartup1['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoStartup1['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoStartup1, UB.GetServoStartup1, pwmLevel, 5)
            if okay:
                self.lblServoStartup1['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoStartup1['fg'] = '#000000'
            else:
                self.lblServoStartup1['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoStartup1['fg'] = '#A00000'

    # Called when butServoMaximum2 is clicked
    def butServoMaximum2_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo2)
        if pwmLevel == 0:
            self.lblServoMaximum2['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMaximum2['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMaximum2, UB.GetServoMaximum2, pwmLevel, 5)
            if okay:
                self.lblServoMaximum2['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMaximum2['fg'] = '#000000'
            else:
                self.lblServoMaximum2['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMaximum2['fg'] = '#A00000'

    # Called when butServoMinimum2 is clicked
    def butServoMinimum2_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo2)
        if pwmLevel == 0:
            self.lblServoMinimum2['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMinimum2['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMinimum2, UB.GetServoMinimum2, pwmLevel, 5)
            if okay:
                self.lblServoMinimum2['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMinimum2['fg'] = '#000000'
            else:
                self.lblServoMinimum2['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMinimum2['fg'] = '#A00000'

    # Called when butServoStartup2 is clicked
    def butServoStartup2_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo2)
        if pwmLevel == 0:
            self.lblServoStartup2['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoStartup2['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoStartup2, UB.GetServoStartup2, pwmLevel, 5)
            if okay:
                self.lblServoStartup2['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoStartup2['fg'] = '#000000'
            else:
                self.lblServoStartup2['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoStartup2['fg'] = '#A00000'

    # Called when butServoMaximum3 is clicked
    def butServoMaximum3_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo3)
        if pwmLevel == 0:
            self.lblServoMaximum3['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMaximum3['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMaximum3, UB.GetServoMaximum3, pwmLevel, 5)
            if okay:
                self.lblServoMaximum3['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMaximum3['fg'] = '#000000'
            else:
                self.lblServoMaximum3['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMaximum3['fg'] = '#A00000'

    # Called when butServoMinimum3 is clicked
    def butServoMinimum3_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo3)
        if pwmLevel == 0:
            self.lblServoMinimum3['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMinimum3['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMinimum3, UB.GetServoMinimum3, pwmLevel, 5)
            if okay:
                self.lblServoMinimum3['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMinimum3['fg'] = '#000000'
            else:
                self.lblServoMinimum3['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMinimum3['fg'] = '#A00000'

    # Called when butServoStartup3 is clicked
    def butServoStartup3_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo3)
        if pwmLevel == 0:
            self.lblServoStartup3['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoStartup3['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoStartup3, UB.GetServoStartup3, pwmLevel, 5)
            if okay:
                self.lblServoStartup3['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoStartup3['fg'] = '#000000'
            else:
                self.lblServoStartup3['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoStartup3['fg'] = '#A00000'

    # Called when butServoMaximum4 is clicked
    def butServoMaximum4_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo4)
        if pwmLevel == 0:
            self.lblServoMaximum4['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMaximum4['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMaximum4, UB.GetServoMaximum4, pwmLevel, 5)
            if okay:
                self.lblServoMaximum4['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMaximum4['fg'] = '#000000'
            else:
                self.lblServoMaximum4['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMaximum4['fg'] = '#A00000'

    # Called when butServoMinimum4 is clicked
    def butServoMinimum4_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo4)
        if pwmLevel == 0:
            self.lblServoMinimum4['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoMinimum4['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoMinimum4, UB.GetServoMinimum4, pwmLevel, 5)
            if okay:
                self.lblServoMinimum4['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoMinimum4['fg'] = '#000000'
            else:
                self.lblServoMinimum4['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoMinimum4['fg'] = '#A00000'

    # Called when butServoStartup4 is clicked
    def butServoStartup4_click(self):
        global UB
        pwmLevel = self.GetLabelValue(self.lblServo4)
        if pwmLevel == 0:
            self.lblServoStartup4['text'] = '%d\nCannot save!' % (pwmLevel)
            self.lblServoStartup4['fg'] = '#A00000'
        else:
            okay = UB.SetWithRetry(UB.SetServoStartup4, UB.GetServoStartup4, pwmLevel, 5)
            if okay:
                self.lblServoStartup4['text'] = '%d\nSaved' % (pwmLevel)
                self.lblServoStartup4['fg'] = '#000000'
            else:
                self.lblServoStartup4['text'] = '%d\nSave failed!' % (pwmLevel)
                self.lblServoStartup4['fg'] = '#A00000'

# if we are the main program (python was passed a script) load the dialog automatically
if __name__ == "__main__":
    app = UltraBorg_tk(None)
    app.mainloop()

Arduino

UltraBorgArduino.ino

// This is a basic example of what the UltraBorg can do
// The example moves each servo at a different rate, wrapping their position when they reach the limit
// This is equivalent to the ubSequence.py example for the Raspberry Pi, except we also read the 
// ultrasonic distances at startup
//
// Before talking to the UltraBorg for the first time we check if we can see the board properly by 
// trying to call UbInit(), if it fails the LED on the Arduino will flash until it can see the board.
// 
// The pins on one of the UltraBoreg six-pin headers need to be connected as follows:
// Pin 1 -> IOREF       The voltage reference for the Arduino I/O, it is marked with a 1
// Pin 2 -> Unused      May be left disconnected
// Pin 3 -> SDA         I2C data line, used for communications
// Pin 4 -> 5V          Power for the PIC, otherwise known as 5v
// Pin 5 -> SCL         I2C clock line, used for communications
// Pin 6 -> GND         Ground, otherwise known as 0v, reference for all other lines
//
// The pins are arranged:
// 1: IOREF   2: Unused
// 3: SDA     4: 5V
// 5: SCL     6: GND
//
// The SDA and SCL connections vary between Arduinos and may not be clearly marked,
// refer to the table below
//
// Board            SDA     SCL
// -----------------------------
// Uno              A4      A5
// Ethernet         A4      A5
// Mega2560         20      21
// Leonardo         2       3
// Due              SDA1    SCL1
//
// If the LED on the Arduino will not stop flashing check the UltraBorg is connected properly (pins as listed above).
// If you are connected and powered but you still cannot get the example to work then uncomment line 72 to
// get the example to scan the I2C bus for the boards address itself, this makes the code slightly larger but
// allows a single board attached to work with any set address.
// See UbSetNewAddress if you wish to change the I2C address used by the UltraBorg.
// If you do change the I2C address change line 72 to set the new address, if using 0x10 for example:
//     Ubi2cAddress = 0x10;
// This way you may also connect multiple UltraBorgs by daisy-chaining them after giving each a unique address, 
// simply set the required address to UbAddress before calling a Ub* function to talk to the required board.
//
// This example has a binary sketch size of 9,120 bytes when compiled for an Arduino Uno using IDE v1.0.5-r2

// Pin 13 has an LED connected on most Arduino boards.
// give it a name:
int led = 13;

#include <Wire.h>               // The I2C library
#include "UltraBorg.h"          // The UltraBorg library

// These are the movement settings
#define SERVO_MIN       -1.0    // Smallest servo position to use
#define SERVO_MAX       +1.0    // Largest servo position to use
#define STARTUP_DELAY   500     // Delay before making the initial move
#define STEP_DELAY      100     // Delay between steps
#define RATE_START      0.05    // Step distance for all servos during initial move
#define RATE_SERVO1     0.01    // Step distance for servo #1
#define RATE_SERVO2     0.02    // Step distance for servo #2
#define RATE_SERVO3     0.04    // Step distance for servo #3
#define RATE_SERVO4     0.08    // Step distance for servo #4

// Runs once
void setup()
{
    Wire.begin();               // Join the I2C bus (we will be acting as master)
    pinMode(led, OUTPUT);       // Initialise the LED pin as an output
    digitalWrite(led, LOW);     // Turn the LED off by making the voltage LOW
    Serial.begin(9600);         // open the serial port at 9600 bps

    // Get the first address of an UltraBorg attached to the I2C bus
    //Ubi2cAddress = UbScanForAddress(0);
}

// Runs looped after setup
void loop()
{        
    float servo1 = 0.0;
    float servo2 = 0.0;
    float servo3 = 0.0;
    float servo4 = 0.0;
    
    Serial.println("Check for UltraBorg...");

    // Check we can see the UltraBorg
    if (UbInit()) {
        Serial.println("UltraBorg connected ^_^");
    } else {
        // Cannot find our board, flash the LED then try again
        Serial.println("No UltraBorg!");
        digitalWrite(led, HIGH);    // Turn the LED on
        delay(250);                 // Delay for a 1/4 second
        digitalWrite(led, LOW);     // Turn the LED off
        delay(250);                 // Delay for a 1/4 second
        return;                     // Skip to the end of the function
    }
    
    // Print the current filtered distance readings (0 means no reading)
    Serial.println("Ultrasonic distances:");
    Serial.println(UbGetDistance1());
    Serial.println(UbGetDistance2());
    Serial.println(UbGetDistance3());
    Serial.println(UbGetDistance4());

    // Set our initial servo positions
    UbSetServoPosition1(servo1);
    UbSetServoPosition2(servo2);
    UbSetServoPosition3(servo3);
    UbSetServoPosition4(servo4);

    // Wait a while to be sure the servos have caught up
    delay(STARTUP_DELAY);
    Serial.println("Sweep to start position");
    while (servo1 > SERVO_MIN) {
        // Reduce the servo positions
        servo1 -= RATE_START;
        servo2 -= RATE_START;
        servo3 -= RATE_START;
        servo4 -= RATE_START;

        // Check if they are too small
        if (servo1 < SERVO_MIN) {
            servo1 = SERVO_MIN;
            servo2 = SERVO_MIN;
            servo3 = SERVO_MIN;
            servo4 = SERVO_MIN;
        }

        // Set our new servo positions
        UbSetServoPosition1(servo1);
        UbSetServoPosition2(servo2);
        UbSetServoPosition3(servo3);
        UbSetServoPosition4(servo4);

        // Wait until the next step
        delay(STEP_DELAY);
    }

    Serial.println("Sweep all servos through the range");
    while (true) {
        // Increase the servo positions at separate rates
        servo1 += RATE_SERVO1;
        servo2 += RATE_SERVO2;
        servo3 += RATE_SERVO3;
        servo4 += RATE_SERVO4;

        // Check if any of them are too large, if so wrap to the over end
        if (servo1 > SERVO_MAX) {
            servo1 -= (SERVO_MAX - SERVO_MIN);
        }
        if (servo2 > SERVO_MAX) {
            servo2 -= (SERVO_MAX - SERVO_MIN);
        }
        if (servo3 > SERVO_MAX) {
            servo3 -= (SERVO_MAX - SERVO_MIN);
        }
        if (servo4 > SERVO_MAX) {
            servo4 -= (SERVO_MAX - SERVO_MIN);
        }

        // Set our new servo positions
        UbSetServoPosition1(servo1);
        UbSetServoPosition2(servo2);
        UbSetServoPosition3(servo3);
        UbSetServoPosition4(servo4);
        
        // Wait until the next step
        delay(STEP_DELAY);
    }
}

UltraBorgTuning.ino

// This is the UltraBorg tuning example for setting up the correct servo ranges using an Arduino
// See https://www.piborg.org/ultraborg/tuning#arduino for operational instructions.
//
// Before talking to the UltraBorg for the first time we check if we can see the board properly by 
// trying to call UbInit(), if it fails the LED on the Arduino will flash until it can see the board.
// 
// The pins on one of the UltraBoreg six-pin headers need to be connected as follows:
// Pin 1 -> IOREF       The voltage reference for the Arduino I/O, it is marked with a 1
// Pin 2 -> Unused      May be left disconnected
// Pin 3 -> SDA         I2C data line, used for communications
// Pin 4 -> 5V          Power for the PIC, otherwise known as 5v
// Pin 5 -> SCL         I2C clock line, used for communications
// Pin 6 -> GND         Ground, otherwise known as 0v, reference for all other lines
//
// The pins are arranged:
// 1: IOREF   2: Unused
// 3: SDA     4: 5V
// 5: SCL     6: GND
//
// The SDA and SCL connections vary between Arduinos and may not be clearly marked,
// refer to the table below
//
// Board            SDA     SCL
// -----------------------------
// Uno              A4      A5
// Ethernet         A4      A5
// Mega2560         20      21
// Leonardo         2       3
// Due              SDA1    SCL1
//
// If the LED on the Arduino will not stop flashing check the UltraBorg is connected properly (pins as listed above).
// If you are connected and powered but you still cannot get the example to work then uncomment line 59 to
// get the example to scan the I2C bus for the boards address itself, this makes the code slightly larger but
// allows a single board attached to work with any set address.
// See UbSetNewAddress if you wish to change the I2C address used by the UltraBorg.
// If you do change the I2C address change line 59 to set the new address, if using 0x10 for example:
//     Ubi2cAddress = 0x10;
// This way you may also connect multiple UltraBorgs by daisy-chaining them after giving each a unique address, 
// simply set the required address to UbAddress before calling a Ub* function to talk to the required board.
//
// This example has a binary sketch size of 10,974 bytes when compiled for an Arduino Uno using IDE v1.0.5-r2

// Pin 13 has an LED connected on most Arduino boards.
// give it a name:
int led = 13;

#include <Wire.h>               // The I2C library
#include "UltraBorg.h"          // The UltraBorg library

// Runs once
void setup()
{
    Wire.begin();               // Join the I2C bus (we will be acting as master)
    pinMode(led, OUTPUT);       // Initialise the LED pin as an output
    digitalWrite(led, LOW);     // Turn the LED off by making the voltage LOW
    Serial.begin(9600);         // open the serial port at 9600 bps

    // Get the first address of an UltraBorg attached to the I2C bus
    //Ubi2cAddress = UbScanForAddress(0);
}

// Function to set a servo position
void setServo(int servo, int position) {
    switch (servo) {
        case 1: UbCalibrateServoPosition1((unsigned int)position); break;
        case 2: UbCalibrateServoPosition2((unsigned int)position); break;
        case 3: UbCalibrateServoPosition3((unsigned int)position); break;
        case 4: UbCalibrateServoPosition4((unsigned int)position); break;
    }
}

// Runs looped after setup
void loop()
{
    int servo;
    int position;
    unsigned int min;
    unsigned int max;
    unsigned int start;
    char command;

    Serial.println("");
    Serial.println("Check for UltraBorg...");

    // Check we can see the UltraBorg
    if (UbInit()) {
        Serial.println("UltraBorg connected ^_^");
    } else {
        // Cannot find our board, flash the LED then try again
        Serial.println("No UltraBorg!");
        digitalWrite(led, HIGH);    // Turn the LED on
        delay(250);                 // Delay for a 1/4 second
        digitalWrite(led, LOW);     // Turn the LED off
        delay(250);                 // Delay for a 1/4 second
        return;                     // Skip to the end of the function
    }

    // Get the servo to work with
    Serial.println("Which servo do you want to tune [1-4] ?");
    while (Serial.available() == 0) delay(100);
    servo = Serial.parseInt();
    if ((servo < 1) || (servo > 4)) {
        Serial.println("The servo number must be between 1 and 4!");
        return;
    }
    Serial.print("Tuning servo #");
    Serial.println(servo);
    Serial.println("");

    // Show the loaded limits
    switch (servo) {
        case 1: min = UbGetServoMinimum1(); break;
        case 2: min = UbGetServoMinimum2(); break;
        case 3: min = UbGetServoMinimum3(); break;
        case 4: min = UbGetServoMinimum4(); break;
    }
    switch (servo) {
        case 1: max = UbGetServoMaximum1(); break;
        case 2: max = UbGetServoMaximum2(); break;
        case 3: max = UbGetServoMaximum3(); break;
        case 4: max = UbGetServoMaximum4(); break;
    }
    switch (servo) {
        case 1: start = UbGetServoStartup1(); break;
        case 2: start = UbGetServoStartup2(); break;
        case 3: start = UbGetServoStartup3(); break;
        case 4: start = UbGetServoStartup4(); break;
    }
    Serial.print("Current settings for servo #");
    Serial.println(servo);
    Serial.print("Minimum: ");
    Serial.println(min);
    Serial.print("Startup: ");
    if ((start == 0x0000) || (start == 0xFFFF)) {
        Serial.print("unset, will use ");
        Serial.println((min + max) >> 1);
    } else {
        Serial.println(start);
    }
    Serial.print("Maximum: ");
    Serial.println(max);
    Serial.println("");

    // Move to central
    Serial.println("Moving to 3000");
    position = 3000;
    setServo(servo, position);

    // Loop to set the maximum
    Serial.println("");
    Serial.print("Setting the maximum for servo #");
    Serial.println(servo);
    Serial.println("");
    Serial.println("Increase the position 100 at a time until the servo no longer moves");
    Serial.println("then decrease the position 10 at a time until the servo moves again");
    Serial.println("Send 'S' when done");
    Serial.println("");
    while (true) {
        // Get the next command
        Serial.print("Position? ");
        while (Serial.available() == 0) delay(100);
        command = Serial.peek();
        if ((command == 'S') || (command == 's')) {
            // Save the maximum position
            Serial.println(command);
            Serial.read();
            switch (servo) {
                case 1: UbSetServoMaximum1((unsigned int)position); break;
                case 2: UbSetServoMaximum2((unsigned int)position); break;
                case 3: UbSetServoMaximum3((unsigned int)position); break;
                case 4: UbSetServoMaximum4((unsigned int)position); break;
            }
            // Read back the maximum position
            Serial.println("");
            Serial.print("Saved - servo #");
            Serial.print(servo);
            Serial.print(" maximum = ");
            switch (servo) {
                case 1: max = UbGetServoMaximum1(); break;
                case 2: max = UbGetServoMaximum2(); break;
                case 3: max = UbGetServoMaximum3(); break;
                case 4: max = UbGetServoMaximum4(); break;
            }
            Serial.println(max);
            Serial.println("");
            break;
        } else {
            // Move to the next position
            position = Serial.parseInt();
            Serial.println(position);
            setServo(servo, position);
        }
    }

    // Move to central
    Serial.println("Moving to 3000");
    position = 3000;
    setServo(servo, position);

    // Loop to set the minimum
    Serial.println("");
    Serial.print("Setting the minimum for servo #");
    Serial.println(servo);
    Serial.println("");
    Serial.println("Decrease the position 100 at a time until the servo no longer moves");
    Serial.println("then increase the position 10 at a time until the servo moves again");
    Serial.println("Send 'S' when done");
    Serial.println("");
    while (true) {
        // Get the next command
        Serial.print("Position? ");
        while (Serial.available() == 0) delay(100);
        command = Serial.peek();
        if ((command == 'S') || (command == 's')) {
            // Save the maximum position
            Serial.println(command);
            Serial.read();
            switch (servo) {
                case 1: UbSetServoMinimum1((unsigned int)position); break;
                case 2: UbSetServoMinimum2((unsigned int)position); break;
                case 3: UbSetServoMinimum3((unsigned int)position); break;
                case 4: UbSetServoMinimum4((unsigned int)position); break;
            }
            // Read back the maximum position
            Serial.println("");
            Serial.print("Saved - servo #");
            Serial.print(servo);
            Serial.print(" minimum = ");
            switch (servo) {
                case 1: min = UbGetServoMinimum1(); break;
                case 2: min = UbGetServoMinimum2(); break;
                case 3: min = UbGetServoMinimum3(); break;
                case 4: min = UbGetServoMinimum4(); break;
            }
            Serial.println(min);
            Serial.println("");
            break;
        } else {
            // Move to the next position
            position = Serial.parseInt();
            Serial.println(position);
            setServo(servo, position);
        }
    }

    // Re-read the new limits into the library
    UbInit();

    // Move to central
    Serial.println("Moving to 3000");
    position = 3000;
    setServo(servo, position);

    // Loop to set the minimum
    Serial.println("");
    Serial.print("Setting the startup for servo #");
    Serial.println(servo);
    Serial.println("");
    Serial.print("Change the position to any place between ");
    Serial.print(min);
    Serial.print(" and ");
    Serial.println(max);
    Serial.println("Send 'S' when done");
    Serial.println("");
    while (true) {
        // Get the next command
        Serial.print("Position? ");
        while (Serial.available() == 0) delay(100);
        command = Serial.peek();
        if ((command == 'S') || (command == 's')) {
            // Save the maximum position
            Serial.println(command);
            Serial.read();
            switch (servo) {
                case 1: UbSetServoStartup1((unsigned int)position); break;
                case 2: UbSetServoStartup2((unsigned int)position); break;
                case 3: UbSetServoStartup3((unsigned int)position); break;
                case 4: UbSetServoStartup4((unsigned int)position); break;
            }
            // Read back the maximum position
            Serial.println("");
            Serial.print("Saved - servo #");
            Serial.print(servo);
            Serial.print(" startup = ");
            switch (servo) {
                case 1: start = UbGetServoStartup1(); break;
                case 2: start = UbGetServoStartup2(); break;
                case 3: start = UbGetServoStartup3(); break;
                case 4: start = UbGetServoStartup4(); break;
            }
            Serial.println(start);
            Serial.println("");
            break;
        } else {
            // Move to the next position
            position = Serial.parseInt();
            Serial.println(position);
            setServo(servo, position);
        }
    }
}

UltraBorg.h

/**************************************/
/******** UltraBorg Constants *********/
/**************************************/

// Commands
// GET commands sent should be followed by a read for the result
// All other commands are send only (no reply)
#define UB_COMMAND_GET_TIME_USM1    (1)     // Get the time measured by ultrasonic #1 in us (0 for no detection)
#define UB_COMMAND_GET_TIME_USM2    (2)     // Get the time measured by ultrasonic #2 in us (0 for no detection)
#define UB_COMMAND_GET_TIME_USM3    (3)     // Get the time measured by ultrasonic #3 in us (0 for no detection)
#define UB_COMMAND_GET_TIME_USM4    (4)     // Get the time measured by ultrasonic #4 in us (0 for no detection)
#define UB_COMMAND_SET_PWM1         (5)     // Set the PWM duty cycle for drive #1 (16 bit)
#define UB_COMMAND_GET_PWM1         (6)     // Get the PWM duty cycle for drive #1 (16 bit)
#define UB_COMMAND_SET_PWM2         (7)     // Set the PWM duty cycle for drive #2 (16 bit)
#define UB_COMMAND_GET_PWM2         (8)     // Get the PWM duty cycle for drive #2 (16 bit)
#define UB_COMMAND_SET_PWM3         (9)     // Set the PWM duty cycle for drive #3 (16 bit)
#define UB_COMMAND_GET_PWM3         (10)    // Get the PWM duty cycle for drive #3 (16 bit)
#define UB_COMMAND_SET_PWM4         (11)    // Set the PWM duty cycle for drive #4 (16 bit)
#define UB_COMMAND_GET_PWM4         (12)    // Get the PWM duty cycle for drive #4 (16 bit)
#define UB_COMMAND_CALIBRATE_PWM1   (13)    // Set the PWM duty cycle for drive #1 (16 bit, ignores limit checks)
#define UB_COMMAND_CALIBRATE_PWM2   (14)    // Set the PWM duty cycle for drive #2 (16 bit, ignores limit checks)
#define UB_COMMAND_CALIBRATE_PWM3   (15)    // Set the PWM duty cycle for drive #3 (16 bit, ignores limit checks)
#define UB_COMMAND_CALIBRATE_PWM4   (16)    // Set the PWM duty cycle for drive #4 (16 bit, ignores limit checks)
#define UB_COMMAND_GET_PWM_MIN_1    (17)    // Get the minimum allowed PWM duty cycle for drive #1
#define UB_COMMAND_GET_PWM_MAX_1    (18)    // Get the maximum allowed PWM duty cycle for drive #1
#define UB_COMMAND_GET_PWM_BOOT_1   (19)    // Get the startup PWM duty cycle for drive #1
#define UB_COMMAND_GET_PWM_MIN_2    (20)    // Get the minimum allowed PWM duty cycle for drive #2
#define UB_COMMAND_GET_PWM_MAX_2    (21)    // Get the maximum allowed PWM duty cycle for drive #2
#define UB_COMMAND_GET_PWM_BOOT_2   (22)    // Get the startup PWM duty cycle for drive #2
#define UB_COMMAND_GET_PWM_MIN_3    (23)    // Get the minimum allowed PWM duty cycle for drive #3
#define UB_COMMAND_GET_PWM_MAX_3    (24)    // Get the maximum allowed PWM duty cycle for drive #3
#define UB_COMMAND_GET_PWM_BOOT_3   (25)    // Get the startup PWM duty cycle for drive #3
#define UB_COMMAND_GET_PWM_MIN_4    (26)    // Get the minimum allowed PWM duty cycle for drive #4
#define UB_COMMAND_GET_PWM_MAX_4    (27)    // Get the maximum allowed PWM duty cycle for drive #4
#define UB_COMMAND_GET_PWM_BOOT_4   (28)    // Get the startup PWM duty cycle for drive #4
#define UB_COMMAND_SET_PWM_MIN_1    (29)    // Set the minimum allowed PWM duty cycle for drive #1
#define UB_COMMAND_SET_PWM_MAX_1    (30)    // Set the maximum allowed PWM duty cycle for drive #1
#define UB_COMMAND_SET_PWM_BOOT_1   (31)    // Set the startup PWM duty cycle for drive #1
#define UB_COMMAND_SET_PWM_MIN_2    (32)    // Set the minimum allowed PWM duty cycle for drive #2
#define UB_COMMAND_SET_PWM_MAX_2    (33)    // Set the maximum allowed PWM duty cycle for drive #2
#define UB_COMMAND_SET_PWM_BOOT_2   (34)    // Set the startup PWM duty cycle for drive #2
#define UB_COMMAND_SET_PWM_MIN_3    (35)    // Set the minimum allowed PWM duty cycle for drive #3
#define UB_COMMAND_SET_PWM_MAX_3    (36)    // Set the maximum allowed PWM duty cycle for drive #3
#define UB_COMMAND_SET_PWM_BOOT_3   (37)    // Set the startup PWM duty cycle for drive #3
#define UB_COMMAND_SET_PWM_MIN_4    (38)    // Set the minimum allowed PWM duty cycle for drive #4
#define UB_COMMAND_SET_PWM_MAX_4    (39)    // Set the maximum allowed PWM duty cycle for drive #4
#define UB_COMMAND_SET_PWM_BOOT_4   (40)    // Set the startup PWM duty cycle for drive #4
#define UB_COMMAND_GET_FILTER_USM1  (41)    // Get the filtered time measured by ultrasonic #1 in us (0 for no detection)
#define UB_COMMAND_GET_FILTER_USM2  (42)    // Get the filtered time measured by ultrasonic #2 in us (0 for no detection)
#define UB_COMMAND_GET_FILTER_USM3  (43)    // Get the filtered time measured by ultrasonic #3 in us (0 for no detection)
#define UB_COMMAND_GET_FILTER_USM4  (44)    // Get the filtered time measured by ultrasonic #4 in us (0 for no detection)
#define UB_COMMAND_GET_ID           (0x99)  // Get the board identifier
#define UB_COMMAND_SET_I2C_ADD      (0xAA)  // Set a new I2C address

// Values
// These are the corresponding numbers for states used by the above commands
#define UB_I2C_ID_SERVO_USM         (0x36)      // I2C values returned when calling the GET_ID command
#define UB_DEFAULT_I2C_ADDRESS      (0x36)      // I2C address set by default (before using SET_I2C_ADD)
#define UB_USM_US_TO_MM             (0.171500)  // Conversion from 'GET_TIME_USM' commands to millimeters
#define UB_PWM_UNSET                (0xFFFF)    // Special value for UbSetServoStartup*, when used resets startup position to be central
#define UB_DELAY_AFTER_EEPROM       (50)        // Delay time after writing EEPROM values before trying to talk again

// Limits
// These define the maximums that the UltraBorg will accept
#define UB_I2C_MAX_LEN              (4)     // Maximum number of bytes in an I2C message
#define UB_DEFAULT_PWM_MAX          (4000)  // Servo maximum limit when set to default 
#define UB_DEFAULT_PWM_MIN          (2000)  // Servo minimum limit when set to default
#define UB_MINIMUM_I2C_ADDRESS      (0x03)  // Minimum allowed value for the I2C address
#define UB_MAXIMUM_I2C_ADDRESS      (0x77)  // Maximum allowed value for the I2C address

/***************************************/
/******** UltraBorg Properties *********/
/***************************************/

// Types
typedef unsigned char byte;                 // Define the term 'byte' if it has not been already

// Values
extern byte Ubi2cAddress;                   // The I2C address we are currently talking to
extern signed long UbServo1Min;             // The current minimum limit for servo #1
extern signed long UbServo1Max;             // The current maximum limit for servo #1
extern signed long UbServo2Min;             // The current minimum limit for servo #2
extern signed long UbServo2Max;             // The current maximum limit for servo #2
extern signed long UbServo3Min;             // The current minimum limit for servo #3
extern signed long UbServo3Max;             // The current maximum limit for servo #3
extern signed long UbServo4Min;             // The current minimum limit for servo #4
extern signed long UbServo4Max;             // The current maximum limit for servo #4

/**************************************/
/******** UltraBorg Functions *********/
/**************************************/

/***** General functions *****/

// Check we are talking to the UltraBorg
bool UbInit(void);

/***** Servo functions *****/

// Sets the drive position for servo output #1
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition1(float position);

// Sets the drive position for servo output #2
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition2(float position);

// Sets the drive position for servo output #3
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition3(float position);

// Sets the drive position for servo output #4
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition4(float position);

// Gets the drive position for servo output #1
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition1(void);

// Gets the drive position for servo output #2
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition2(void);

// Gets the drive position for servo output #3
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition3(void);

// Gets the drive position for servo output #4
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition4(void);

/***** Ultrasonic functions *****/

// Gets the filtered distance for ultrasonic module #1 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance1 instead (no filtering)
float UbGetDistance1(void);

// Gets the filtered distance for ultrasonic module #2 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance2 instead (no filtering)
float UbGetDistance2(void);

// Gets the filtered distance for ultrasonic module #3 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance3 instead (no filtering)
float UbGetDistance3(void);

// Gets the filtered distance for ultrasonic module #4 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance4 instead (no filtering)
float UbGetDistance4(void);

// Gets the raw distance for ultrasonic module #1 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance1
float UbGetRawDistance1(void);

// Gets the raw distance for ultrasonic module #2 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance2
float UbGetRawDistance2(void);

// Gets the raw distance for ultrasonic module #3 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance3
float UbGetRawDistance3(void);

// Gets the raw distance for ultrasonic module #4 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance4
float UbGetRawDistance4(void);

/***** Startup functions *****/

// Gets the startup PWM level for servo output #1
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup1(void);

// Gets the startup PWM level for servo output #2
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup2(void);

// Gets the startup PWM level for servo output #3
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup3(void);

// Gets the startup PWM level for servo output #4
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup4(void);

// Sets the startup PWM level for servo output #1
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup1(unsigned int pwmLevel);

// Sets the startup PWM level for servo output #2
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup2(unsigned int pwmLevel);

// Sets the startup PWM level for servo output #3
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup3(unsigned int pwmLevel);

// Sets the startup PWM level for servo output #4
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup4(unsigned int pwmLevel);

/***** Limit functions *****/

// Gets the minimum PWM level for servo output #1
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum1(void);

// Gets the minimum PWM level for servo output #2
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum2(void);

// Gets the minimum PWM level for servo output #3
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum3(void);

// Gets the minimum PWM level for servo output #4
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum4(void);

// Gets the maximum PWM level for servo output #1
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum1(void);

// Gets the maximum PWM level for servo output #2
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum2(void);

// Gets the maximum PWM level for servo output #3
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum3(void);

// Gets the maximum PWM level for servo output #4
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum4(void);

// Sets the raw PWM level for servo output #1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition1(unsigned int pwmLevel);

// Sets the raw PWM level for servo output #2
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition2(unsigned int pwmLevel);

// Sets the raw PWM level for servo output #3
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition3(unsigned int pwmLevel);

// Sets the raw PWM level for servo output #4
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition4(unsigned int pwmLevel);

// Gets the raw PWM level for servo output #1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition1
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition1(void);

// Gets the raw PWM level for servo output #2
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition2
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition2(void);

// Gets the raw PWM level for servo output #3
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition3
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition3(void);

// Gets the raw PWM level for servo output #4
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition4
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition4(void);

// Sets the minimum PWM level for servo output #1
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum1(unsigned int pwmLevel);

// Sets the minimum PWM level for servo output #2
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum2(unsigned int pwmLevel);

// Sets the minimum PWM level for servo output #3
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum3(unsigned int pwmLevel);

// Sets the minimum PWM level for servo output #1
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum4(unsigned int pwmLevel);

// Sets the maximum PWM level for servo output #1
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum1(unsigned int pwmLevel);

// Sets the maximum PWM level for servo output #2
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum2(unsigned int pwmLevel);

// Sets the maximum PWM level for servo output #3
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum3(unsigned int pwmLevel);

// Sets the maximum PWM level for servo output #4
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum4(unsigned int pwmLevel);

/***** Advanced functions *****/

// Scans the I2C bus for UltraBorg boards and returns a count of all the boards found
byte UbScanForCount(void);

// Scans the I2C bus for an UltraBorg board, index is which address to return (from 0 to count - 1)
// Returns address 0 if no board is found for that index
byte UbScanForAddress(byte index);

// Sets the UltraBorg at the current address to newAddress
// Warning, this new I²C address will still be used after resetting the power on the device
// If successful returns true and updates Ubi2cAddress, otherwise returns false
bool UbSetNewAddress(byte newAddress);

UltraBorg.cpp

/**************************************/
/********* UltraBorg Library **********/
/**************************************/

// Includes
#if defined(ARDUINO) && ARDUINO >= 100
    #include "Arduino.h"
#else
    #include "WProgram.h"
#endif
#include <Wire.h>               // The I2C library
#include "UltraBorg.h"          // The UltraBorg library constants

// Parameters
byte Ubi2cAddress = UB_DEFAULT_I2C_ADDRESS;     // The I2C address we are currently talking to

// Private memory
byte rdBuffer[UB_I2C_MAX_LEN];                  // Buffer used for reading replies
signed long UbServo1Min = UB_DEFAULT_PWM_MIN;  // The current minimum limit for servo #1
signed long UbServo1Max = UB_DEFAULT_PWM_MAX;  // The current maximum limit for servo #1
signed long UbServo2Min = UB_DEFAULT_PWM_MIN;  // The current minimum limit for servo #2
signed long UbServo2Max = UB_DEFAULT_PWM_MAX;  // The current maximum limit for servo #2
signed long UbServo3Min = UB_DEFAULT_PWM_MIN;  // The current minimum limit for servo #3
signed long UbServo3Max = UB_DEFAULT_PWM_MAX;  // The current maximum limit for servo #3
signed long UbServo4Min = UB_DEFAULT_PWM_MIN;  // The current minimum limit for servo #4
signed long UbServo4Max = UB_DEFAULT_PWM_MAX;  // The current maximum limit for servo #4

// Private function used to read the reply to GET commands
// Overwrites the contents of rdBuffer
void ReadInReply(void) {
    int idx;
    Wire.requestFrom(Ubi2cAddress, (byte)UB_I2C_MAX_LEN);
    for (idx = 0; idx < UB_I2C_MAX_LEN; ++idx) {
        if (Wire.available()) {
            rdBuffer[idx] = Wire.read();
        } else {
            rdBuffer[idx] = 0;
        }
    }
}

/**************************************/
/******** UltraBorg Functions *********/
/**************************************/

/***** General functions *****/

// Check we are talking to the UltraBorg
bool UbInit(void) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_ID);
    Wire.endTransmission();
    ReadInReply();

    if (rdBuffer[1] == UB_I2C_ID_SERVO_USM) {
        // Read in the limits for this board
        UbServo1Min = UbGetServoMinimum1();
        UbServo1Max = UbGetServoMaximum1();
        UbServo2Min = UbGetServoMinimum2();
        UbServo2Max = UbGetServoMaximum2();
        UbServo3Min = UbGetServoMinimum3();
        UbServo3Max = UbGetServoMaximum3();
        UbServo4Min = UbGetServoMinimum4();
        UbServo4Max = UbGetServoMaximum4();

        return true;
    } else {
        return false;
    }
}

/***** Servo functions *****/

// Sets the drive position for servo output #1
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition1(float position) {
    float powerOut = (position + 1.0) / 2.0;
    unsigned int pwmDuty = (unsigned int)((powerOut * (UbServo1Max - UbServo1Min)) + UbServo1Min);
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM1);
    Wire.write((byte)((pwmDuty >> 8) & 0xFF));
    Wire.write((byte)(pwmDuty & 0xFF));
    Wire.endTransmission();
}

// Sets the drive position for servo output #2
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition2(float position) {
    float powerOut = (position + 1.0) / 2.0;
    unsigned int pwmDuty = (unsigned int)((powerOut * (UbServo2Max - UbServo2Min)) + UbServo2Min);
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM2);
    Wire.write((byte)((pwmDuty >> 8) & 0xFF));
    Wire.write((byte)(pwmDuty & 0xFF));
    Wire.endTransmission();
}

// Sets the drive position for servo output #3
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition3(float position) {
    float powerOut = (position + 1.0) / 2.0;
    unsigned int pwmDuty = (unsigned int)((powerOut * (UbServo3Max - UbServo3Min)) + UbServo3Min);
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM3);
    Wire.write((byte)((pwmDuty >> 8) & 0xFF));
    Wire.write((byte)(pwmDuty & 0xFF));
    Wire.endTransmission();
}

// Sets the drive position for servo output #4
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition4(float position) {
    float powerOut = (position + 1.0) / 2.0;
    unsigned int pwmDuty = (unsigned int)((powerOut * (UbServo4Max - UbServo4Min)) + UbServo4Min);
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM4);
    Wire.write((byte)((pwmDuty >> 8) & 0xFF));
    Wire.write((byte)(pwmDuty & 0xFF));
    Wire.endTransmission();
}

// Gets the drive position for servo output #1
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition1(void) {
    unsigned int pwmDuty;
    float powerOut;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM1);
    Wire.endTransmission();
    ReadInReply();
    pwmDuty = (rdBuffer[1] << 8) + rdBuffer[2];
    powerOut = ((float)pwmDuty - UbServo1Min) / (UbServo1Max - UbServo1Min);
    return (2.0 * powerOut) - 1.0;
}

// Gets the drive position for servo output #2
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition2(void) {
    unsigned int pwmDuty;
    float powerOut;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM2);
    Wire.endTransmission();
    ReadInReply();
    pwmDuty = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    powerOut = ((float)pwmDuty - UbServo2Min) / (UbServo2Max - UbServo2Min);
    return (2.0 * powerOut) - 1.0;
}

// Gets the drive position for servo output #3
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition3(void) {
    unsigned int pwmDuty;
    float powerOut;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM3);
    Wire.endTransmission();
    ReadInReply();
    pwmDuty = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    powerOut = ((float)pwmDuty - UbServo3Min) / (UbServo3Max - UbServo3Min);
    return (2.0 * powerOut) - 1.0;
}

// Gets the drive position for servo output #4
// 0 is central, -1 is maximum left, +1 is maximum right
float UbGetServoPosition4(void) {
    unsigned int pwmDuty;
    float powerOut;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM4);
    Wire.endTransmission();
    ReadInReply();
    pwmDuty = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    powerOut = ((float)pwmDuty - UbServo4Min) / (UbServo4Max - UbServo4Min);
    return (2.0 * powerOut) - 1.0;
}

/***** Ultrasonic functions *****/

// Gets the filtered distance for ultrasonic module #1 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance1 instead (no filtering)
float UbGetDistance1(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_FILTER_USM1);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the filtered distance for ultrasonic module #2 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance2 instead (no filtering)
float UbGetDistance2(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_FILTER_USM2);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the filtered distance for ultrasonic module #3 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance3 instead (no filtering)
float UbGetDistance3(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_FILTER_USM3);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
};

// Gets the filtered distance for ultrasonic module #4 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// If you need a faster response try UbGetRawDistance4 instead (no filtering)
float UbGetDistance4(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_FILTER_USM4);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the raw distance for ultrasonic module #1 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance1
float UbGetRawDistance1(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_TIME_USM1);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the raw distance for ultrasonic module #2 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance2
float UbGetRawDistance2(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_TIME_USM2);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the raw distance for ultrasonic module #3 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance3
float UbGetRawDistance3(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_TIME_USM3);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

// Gets the raw distance for ultrasonic module #4 in millimeters
// Returns 0 for no object detected or no ultrasonic module attached
// For a filtered (less jumpy) reading use UbGetDistance4
float UbGetRawDistance4(void) {
    unsigned int time_us;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_TIME_USM4);
    Wire.endTransmission();
    ReadInReply();
    time_us = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    if (time_us == 65535) time_us = 0;
    return (float)time_us * UB_USM_US_TO_MM;
}

/***** Startup functions *****/

// Gets the startup PWM level for servo output #1
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup1(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_BOOT_1);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the startup PWM level for servo output #2
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup2(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_BOOT_2);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the startup PWM level for servo output #3
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup3(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_BOOT_3);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the startup PWM level for servo output #4
// This can be anywhere in the minimum to maximum range
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoStartup4(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_BOOT_4);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Sets the startup PWM level for servo output #1
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup1(unsigned int pwmLevel) {
    bool inRange = true;
    if (UbServo1Min < UbServo1Max) {
        // Normal direction
        if (pwmLevel < UbServo1Min) {
            inRange = false;
        } else if (pwmLevel > UbServo1Max) {
            inRange = false;
        }
    } else {
        // Inverted direction
        if (pwmLevel > UbServo1Min) {
            inRange = false;
        } else if (pwmLevel < UbServo1Max) {
            inRange = false;
        }
    }
    if (pwmLevel == UB_PWM_UNSET) {
        // Force to unset behaviour (central)
        inRange = true;
    }

    if (inRange) {
        Wire.beginTransmission(Ubi2cAddress);
        Wire.write(UB_COMMAND_SET_PWM_BOOT_1);
        Wire.write((byte)((pwmLevel >> 8) & 0xFF));
        Wire.write((byte)(pwmLevel & 0xFF));
        Wire.endTransmission();
        delay(UB_DELAY_AFTER_EEPROM);
    }
    return inRange;
}

// Sets the startup PWM level for servo output #2
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup2(unsigned int pwmLevel) {
    bool inRange = true;
    if (UbServo2Min < UbServo2Max) {
        // Normal direction
        if (pwmLevel < UbServo2Min) {
            inRange = false;
        } else if (pwmLevel > UbServo2Max) {
            inRange = false;
        }
    } else {
        // Inverted direction
        if (pwmLevel > UbServo2Min) {
            inRange = false;
        } else if (pwmLevel < UbServo2Max) {
            inRange = false;
        }
    }
    if (pwmLevel == UB_PWM_UNSET) {
        // Force to unset behaviour (central)
        inRange = true;
    }

    if (inRange) {
        Wire.beginTransmission(Ubi2cAddress);
        Wire.write(UB_COMMAND_SET_PWM_BOOT_2);
        Wire.write((byte)((pwmLevel >> 8) & 0xFF));
        Wire.write((byte)(pwmLevel & 0xFF));
        Wire.endTransmission();
        delay(UB_DELAY_AFTER_EEPROM);
    }
    return inRange;
}

// Sets the startup PWM level for servo output #3
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup3(unsigned int pwmLevel) {
    bool inRange = true;
    if (UbServo3Min < UbServo3Max) {
        // Normal direction
        if (pwmLevel < UbServo3Min) {
            inRange = false;
        } else if (pwmLevel > UbServo3Max) {
            inRange = false;
        }
    } else {
        // Inverted direction
        if (pwmLevel > UbServo3Min) {
            inRange = false;
        } else if (pwmLevel < UbServo3Max) {
            inRange = false;
        }
    }
    if (pwmLevel == UB_PWM_UNSET) {
        // Force to unset behaviour (central)
        inRange = true;
    }

    if (inRange) {
        Wire.beginTransmission(Ubi2cAddress);
        Wire.write(UB_COMMAND_SET_PWM_BOOT_3);
        Wire.write((byte)((pwmLevel >> 8) & 0xFF));
        Wire.write((byte)(pwmLevel & 0xFF));
        Wire.endTransmission();
        delay(UB_DELAY_AFTER_EEPROM);
    }
    return inRange;
}

// Sets the startup PWM level for servo output #4
// This can be anywhere in the minimum to maximum range
// 
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// This value is checked against the current servo limits before setting
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
bool UbSetServoStartup4(unsigned int pwmLevel) {
    bool inRange = true;
    if (UbServo4Min < UbServo4Max) {
        // Normal direction
        if (pwmLevel < UbServo4Min) {
            inRange = false;
        } else if (pwmLevel > UbServo4Max) {
            inRange = false;
        }
    } else {
        // Inverted direction
        if (pwmLevel > UbServo4Min) {
            inRange = false;
        } else if (pwmLevel < UbServo4Max) {
            inRange = false;
        }
    }
    if (pwmLevel == UB_PWM_UNSET) {
        // Force to unset behaviour (central)
        inRange = true;
    }

    if (inRange) {
        Wire.beginTransmission(Ubi2cAddress);
        Wire.write(UB_COMMAND_SET_PWM_BOOT_4);
        Wire.write((byte)((pwmLevel >> 8) & 0xFF));
        Wire.write((byte)(pwmLevel & 0xFF));
        Wire.endTransmission();
        delay(UB_DELAY_AFTER_EEPROM);
    }
    return inRange;
}

/***** Limit functions *****/

// Gets the minimum PWM level for servo output #1
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum1(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MIN_1);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the minimum PWM level for servo output #2
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum2(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MIN_2);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the minimum PWM level for servo output #3
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum3(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MIN_3);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the minimum PWM level for servo output #4
// This corresponds to position -1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMinimum4(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MIN_4);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the maximum PWM level for servo output #1
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum1(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MAX_1);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the maximum PWM level for servo output #2
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum2(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MAX_2);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the maximum PWM level for servo output #3
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum3(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MAX_3);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the maximum PWM level for servo output #4
// This corresponds to position +1
// The value is an integer where 2000 represents a 1 ms servo burst
unsigned int UbGetServoMaximum4(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM_MAX_4);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Sets the raw PWM level for servo output #1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition1(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_CALIBRATE_PWM1);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
}

// Sets the raw PWM level for servo output #2
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition2(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_CALIBRATE_PWM2);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
}

// Sets the raw PWM level for servo output #3
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition3(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_CALIBRATE_PWM3);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
}

// Sets the raw PWM level for servo output #4
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// NO LIMIT CHECKING IS PERFORMED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbCalibrateServoPosition4(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_CALIBRATE_PWM4);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
}

// Gets the raw PWM level for servo output #1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition1
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition1(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM1);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the raw PWM level for servo output #2
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition2
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition2(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM2);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the raw PWM level for servo output #3
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition3
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition3(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM3);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Gets the raw PWM level for servo output #4
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// This value requires interpreting into an actual servo position, this is already done by UbGetServoPosition4
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
unsigned int UbGetRawServoPosition4(void) {
    unsigned int pwmLevel;
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_GET_PWM4);
    Wire.endTransmission();
    ReadInReply();
    pwmLevel = ((unsigned int)rdBuffer[1] << 8) + (unsigned int)rdBuffer[2];
    return pwmLevel;
}

// Sets the minimum PWM level for servo output #1
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum1(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MIN_1);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo1Min = UbGetServoMinimum1();
}

// Sets the minimum PWM level for servo output #2
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum2(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MIN_2);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo2Min = UbGetServoMinimum2();
}

// Sets the minimum PWM level for servo output #3
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum3(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MIN_3);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo3Min = UbGetServoMinimum3();
}

// Sets the minimum PWM level for servo output #1
// This corresponds to position -1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMinimum4(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MIN_4);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo4Min = UbGetServoMinimum4();
}

// Sets the maximum PWM level for servo output #1
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition1 / UbGetServoPosition1
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum1(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MAX_1);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo1Min = UbGetServoMinimum1();
}

// Sets the maximum PWM level for servo output #2
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition2 / UbGetServoPosition2
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum2(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MAX_2);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo2Min = UbGetServoMinimum2();
}

// Sets the maximum PWM level for servo output #3
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition3 / UbGetServoPosition3
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum3(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MAX_3);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo3Min = UbGetServoMinimum3();
}

// Sets the maximum PWM level for servo output #4
// This corresponds to position +1
// This value can be set anywhere from 0 for a 0% duty cycle to 65535 for a 100% duty cycle
// 
// Setting values outside the range of the servo for extended periods of time can damage the servo
// LIMIT CHECKING IS ALTERED BY THIS COMMAND!
// We recommend using the tuning GUI for setting the servo limits for UbSetServoPosition4 / UbGetServoPosition4
// 
// The value is an integer where 2000 represents a 1ms servo burst, approximately 3% duty cycle
void UbSetServoMaximum4(unsigned int pwmLevel) {
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_PWM_MAX_4);
    Wire.write((byte)((pwmLevel >> 8) & 0xFF));
    Wire.write((byte)(pwmLevel & 0xFF));
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    UbServo4Min = UbGetServoMinimum4();
}

/***** Advanced functions *****/

// Scans the I2C bus for UltraBorg boards and returns a count of all the boards found
byte UbScanForCount(void) {
    byte found = 0;
    byte oldAddress = Ubi2cAddress;
    for (Ubi2cAddress = UB_MINIMUM_I2C_ADDRESS; Ubi2cAddress <= UB_MAXIMUM_I2C_ADDRESS; ++Ubi2cAddress) {
        if (UbInit()) {
            ++found;
        }
    }
    Ubi2cAddress = oldAddress;
    return found;
}

// Scans the I2C bus for an UltraBorg board, index is which address to return (from 0 to count - 1)
// Returns address 0 if no board is found for that index
byte UbScanForAddress(byte index) {
    byte found = 0;
    byte oldAddress = Ubi2cAddress;
    for (Ubi2cAddress = UB_MINIMUM_I2C_ADDRESS; Ubi2cAddress <= UB_MAXIMUM_I2C_ADDRESS; ++Ubi2cAddress) {
        if (UbInit()) {
            if (index == 0) {
                found = Ubi2cAddress;
                break;
            } else {
                --index;
            }
        }
    }
    Ubi2cAddress = oldAddress;
    return found;
}

// Sets the UltraBorg at the current address to newAddress
// Warning, this new I²C address will still be used after resetting the power on the device
// If successful returns true and updates Ubi2cAddress, otherwise returns false
bool UbSetNewAddress(byte newAddress) {
    byte oldAddress = Ubi2cAddress;
    if (newAddress < UB_MINIMUM_I2C_ADDRESS) {
        return false;
    } else if (newAddress > UB_MAXIMUM_I2C_ADDRESS) {
        return false;
    } else if (!UbInit()) {
        return false;
    }
    Wire.beginTransmission(Ubi2cAddress);
    Wire.write(UB_COMMAND_SET_I2C_ADD);
    Wire.write(newAddress);
    Wire.endTransmission();
    delay(UB_DELAY_AFTER_EEPROM);
    Ubi2cAddress = newAddress;
    if (UbInit()) {
        return true;
    } else {
        Ubi2cAddress = oldAddress;
        return false;
    }
}