In this post, I finally put my hands on the RpiSOC board to make some experiments with servos, in particular with easing functions. My purpose is to control servos speed and acceleration by sending high-level command (i.e. move to 120 degrees position) from the Raspberry PI board and let the RpiSOC generate the PWM to make the movement as smooth as possible. I like C/C++ language like no other programming language, so I'm not going to use the provided Python interfaces but I will use PSOC Creator instead
RpiSOC overview
After playing for some time with RpiSOC I have only one terms that describes this board: unbeliavable!
With a free and incredibly user-friendly application (PSOC Creator) you can exploit all the power of the hardware resources without bothering at all about registers.
PSOC Creator
PSOC Creator has two user interfaces.
The first one let's you to graphically configure hardware resources. When you drag-and-drop a component (i.e. a PWM or an ADC), the application generates all the C code required to configure and act on that component
The second interface is a C IDE that let you call the auto-generated C functions to build the application logic
You can find an introductory video in this tutorial
Controlling servos
Currently I use the pigpio library to control servos. When the function to move a servo is called, the library generates the PWM corresponding to the final position. The control logic inside the servo activate the motor at the maximum speed until the final position is reached
I'd like to make to movement more smooth, but this will require the Raspberry PI board to change PWM according to a certain speed profile. This is well beyond the real-time capabilities of the Linux operating system, but it's perfectly matched by a microprocessor programmed at bare-metal level
Before proceeding with implementation details, let's first introduce some concepts about controlling servos and speed profiles.
As the name implies, speed profiles depict how servo speed changes with time. However servos are controlled by generating not a speed signal but a position signal. Position can be obtained from speed profile by interpolating speed over time (i.e. summing up, for each time interval, the speed multiplied by time interval)
Trapezoidal speed profile
One of the most famous speed profile is the trapezoid. The profile is made up of three phases: acceleration, constant speed and deceleration
So the function that describe speed in terms of time is
The position in the three phases can then be calculated as
Penner easing functions
I made some other tests to smooth the servo movements using classic easing functions. This are typically used for animations, but I just wanted to experiment something different...
The generic form of the Penner easing function is
The "class" defines how fast the easing functions is at start. Typically, 5 classes are used
- Linear (class = 1): any number to the power of 1 is itself, so at 10% of the way through the tween, the property will have added 10% of the total difference between start and end
- Quad (class = 2)
- Cubic (class = 3)
- Quart (class = 4)
- Quint and Strong (class = 5)
Implementation
I implemented servo easing using PSoc Creator. First I created a easing module that wraps the logic of the easing itself. The second step was the implementation of the communication between Raspberry PI and RpiSOC through I2C interface
I will just spend some words about the software code, because the hardware configuration can easily be undestood by opening the attached source code in PSocCreator
Easing library
The easing library exposes just four functions
void easing_start();
This function is invoked to initialized to library. After initialization, easing library must be made aware of the current servo positions. This is accomplished by invoking the function
void easing_setValue(uint8 servo, double value);
where
servo identifies the servo. Valid values are in the range 0.. EASING_SERVO_MAX-1
value is the current position of the servo (in degrees)
To move a servo, the following function is invoked
void easing_initStep(uint8 servo,
double end,
uint16 duration,
uint8 easing,
void* settings);
where
servo identifies the servo. Valid values are in the range 0.. EASING_SERVO_MAX-1
end is the position the servo has to be moved to
duration is duration of the movement in milliseconds
easing is the easing function to apply. Valid values are EASING_TRAPEZOID and EASING_PENNER
settings is a pointer to a structure that depends on the value of the easing parameter. If easing is EASING_TRAPEZOID then the following structure is used
typedef struct
{
uint16 timeUp;
uint16 timeDown;
} SETTINGS_TRAPEZOID;
where timeUp and timeDown are the durations (in milliseconds) of the acceleration and deceleration phases.
If easing is EASING_PENNER then the following structure is used
typedef struct
{
uint8 power;
} SETTINGS_PENNER;
where power is the class of the Penner function
Last function makes the library calculate the next position of the servo.
double easing_next(uint8 servo, uint16 dt, uint8* completed);
servo identifies the servo. Valid values are in the range 0.. EASING_SERVO_MAX-1
dt is the time elapsed (in milliseconds) since the last invocation of the easing_next function
completed is an output parameter that returns 1 when the servo has reached the final position
Communication
Raspberry PI sends command to RpiSOC using the I2C interface. The communication protocol has currently just in command.
Offset | Description |
0 | number of bytes following |
1 | Command identifier |
2 | flags (currently no flags are defined) |
3 | duration of the movements in tenth of seconds |
4 | number of servos to controls |
5 | servo identifier (0.. EASING_SERVO_MAX-1) |
6 | final angle value (MSB) in 100 of degress |
7 | final angle value (LSB) in 100 of degress |
8 | reserved |
9 | reserved |
* bytes from 5 to 9 repeats for the number of servos specified in byte 4
Raspberry PI application
I finally created a sample application on Raspberry PI to continuously send a command to the RpiSOC and see that the servo is properly controlled.
The communication on I2C has been implemented using the i2c_smbus_*() functions
The application is very simple and is just for testing purposes
Before starting, you need to be sure that I2C drivers are loaded and I2C libraries are installed. This tutorial is very clear
First of all, I open the I2C device
// Open I2C device
if ((fd = open(device, O_RDWR)) < 0)
exit_on_error ("Can't open I2C device");
if (ioctl(fd, I2C_FUNCS, &functionality) < 0)
exit_on_error ("Can't use I2C_FUNCS ioctl");
The I set the I2C slave address (must match the address you configure on RpiSOC)
if (ioctl(fd, I2C_SLAVE, 8) < 0)
exit_on_error ("Can't set slave address");
and finally I send out the command
i2c_smbus_write_i2c_block_data ( fd , 0x01, bufferLen, &buffer[0] );
To properly release the I2C interface, I just call
close(fd);
