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.
1
2
3
4
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.
1
2
3
4
5
6
7
#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.
1
2
3
4
5
6
7
8
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:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#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.
1
2
3
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:
1
2
3
4
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:
1
2
3
4
5
6
7
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.
1
2
3
4
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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471
1472
1473
1474
1475
1476
1477
1478
1479
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498
1499
1500
1501
1502
1503
1504
1505
1506
1507
1508
1509
1510
1511
1512
1513
1514
1515
1516
1517
1518
1519
1520
1521
1522
1523
1524
1525
1526
1527
1528
1529
1530
1531
1532
1533
1534
1535
1536
1537
1538
1539
1540
1541
1542
1543
1544
1545
1546
1547
1548
1549
1550
1551
1552
1553
1554
1555
1556
1557
1558
1559
1560
1561
1562
1563
1564
1565
1566
1567
1568
1569
1570
1571
1572
1573
1574
1575
1576
1577
1578
1579
1580
1581
1582
1583
1584
1585
1586
1587
1588
1589
1590
1591
1592
1593
1594
1595
1596
1597
1598
1599
1600
1601
1602
1603
1604
1605
1606
1607
1608
1609
1610
1611
1612
1613
1614
1615
1616
1617
1618
1619
1620
1621
1622
1623
1624
1625
1626
1627
1628
1629
1630
1631
1632
1633
1634
1635
1636
1637
1638
1639
1640
1641
1642
1643
1644
1645
1646
1647
1648
1649
1650
1651
1652
1653
1654
1655
1656
1657
1658
1659
1660
1661
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675
1676
1677
1678
1679
1680
1681
1682
1683
1684
1685
1686
1687
1688
1689
1690
1691
1692
1693
1694
1695
1696
1697
1698
1699
1700
1701
1702
1703
1704
1705
1706
1707
1708
1709
1710
1711
1712
1713
1714
1715
1716
1717
1718
1719
1720
1721
1722
1723
1724
1725
1726
1727
1728
1729
1730
1731
1732
1733
1734
1735
1736
1737
1738
1739
1740
1741
1742
1743
1744
1745
1746
1747
1748
1749
1750
1751
1752
1753
1754
1755
1756
1757
1758
1759
1760
1761
1762
1763
1764
1765
1766
1767
1768
1769
1770
1771
1772
1773
1774
1775
1776
1777
1778
1779
1780
1781
1782
1783
1784
1785
1786
1787
1788
1789
1790
1791
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
// 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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
// 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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
/**************************************/
/******** 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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
/**************************************/
/********* 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;
    }
}
Subscribe to Comments for &quot;UltraBorg - Precision servo control with ultrasonic module support&quot;