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

SpeedProfile.png

 

So the function that describe speed in terms of time is

 

Formula1.png 

 

The position in the three phases can then be calculated as

Formula2.png

 

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

  Formula3.png

 

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
0x01 = start movement

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);