Overview

In this lesson, we’ll show how to control SG90 servo using Raspberry Pi.

Hareware Preparation

1 * Raspberry Pi

1 * Breadboard

1 * SG90 servo motor

1 * PCA9685

1 * Breadboard power supply

Jumper wires

1 * T-Extension Board

1 * 40-Pin Cable

Software Preparation
Note: In this lesson, we remotely control raspberry pi via PuTTy on PC. To learn how to config raspberry pi, please visit lesson 1: getting started with raspberry pi.

Work principle

A SG90 is a servo which can rotate approximately 180 degrees.  It consists of a DC-motor, position system and gears. there are three wires: the red wire is connected to power, the brown wire is connected to Ground, the yellow wire is connected to PWM Signal. The rotate angle is determined by the equation:

DutyCycle=PulseWidth/Period

Period=1/Frequency

so

DutyCycle=PulseWidth/(1/Frequency)=PulseWidth*Frequency

In this project, we use a PCA9685 16-channel servo motor driver module to control the Dutycycle. The PCA9685 module is an i2c-controlled PWM driver, it uses only two pins to control 16 PWM outputs. You can chain up 62 same modules on a single i2c at once. That means can control up to total of 992 outputs. the PCA9685 work frequence is adjustable from 24Hz to 1526Hz, the dutycycle is adjustable from0% to 100%, for PCA9685 datasheet, please visit here:datasheet

 Schematic diagram as followed:

Hardware Setup

Sample code

Before running program, please config the Pi as followed steps:

1) Enable I2C(if you have done, please skip)

sudo  nano  /boot/config.txt

open the file /boot/config.txt, find the code line”dtparam=i2c_arm”,checking if there is # sign in front of the line, uncomment it (remove the # in front of this line), finally the code should looks like this:

2) Load IIC Modules(if you have done, please skip)

sudo nano /etc/modules

open /etc/modules file,Add these two lines as below:

i2c-bcm2708
i2c-dev

3) Reboot Pi

reboot


for C language users

1 ) Download and unzip the sample code file pca9685-servo.tar

cd  ~

wget  http://osoyoo.com/driver/pi3_start_learning_kit_lesson_12/pca9685-servo.tar.gz

sudo  tar  xzvf  pca9685-servo.tar.gz

2) Change the path

cd  /home/pi/pca9685-servo

3) Compile code

gcc  -o servo servo.c  pca9685.c  -lwiringPi

4) Run program

sudo  ./servo

5) Test result

Once the program is running, it will rotate to a random angles.
Sample code and analysis comments:

servo.c
 
  1. #include "pca9685.h"
  2. #include < wiringPi.h>
  3. #include < stdio.h>
  4. #include < stdlib.h>
  5. #define PIN_BASE 300
  6. #define MAX_PWM 4096
  7. #define HERTZ 50
  8. /**
  9. * Calculate the number of ticks the signal should be high for the required amount of time
  10. */
  11. int calcTicks(float impulseMs, int hertz)
  12. {
  13.       float cycleMs = 1000.0f / hertz;
  14.       return (int)(MAX_PWM * impulseMs / cycleMs + 0.5f);
  15. }
  16. /**
  17. * input is [0..1]
  18. * output is [min..max]
  19. */
  20. float map(float input, float min, float max)
  21. {
  22.       return (input * max) + (1 - input) * min;
  23. }
  24. int main(void)
  25. {
  26.       printf("PCA9685 servo example\n");
  27.       printf("Connect a servo to any pin. It will rotate to random angles\n");
  28.       // Setup with pinbase 300 and i2c location 0x40
  29.       int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
  30.       if (fd < 0)
  31.       {
  32.             printf("Error in setup\n");
  33.             return fd;
  34.       }
  35.       // Reset all output
  36.       pca9685PWMReset(fd);
  37.       // Set servo to neutral position at 1.5 milliseconds
  38.       // (View http://en.wikipedia.org/wiki/Servo_control#Pulse_duration)
  39.       float millis = 1.5;
  40.       int tick = calcTicks(millis, HERTZ);
  41.       pwmWrite(PIN_BASE + 16, tick);
  42.       delay(2000);
  43.       int active = 1;
  44.       while (active)
  45.       {
  46.             // That's a hack. We need a random number < 1             float r = rand();             while (r > 1)
  47.                   r /= 10;
  48.             millis = map(r, 1, 2);
  49.             tick = calcTicks(millis, HERTZ);
  50.             pwmWrite(PIN_BASE + 16, tick);
  51.             delay(1000);
  52.       }
  53.       return 0;
  54. }

pca9685.c
 
  1. #include < wiringPi.h >
  2. #include < wiringPiI2C.h >
  3. #include "pca9685.h"
  4. // Setup registers
  5. #define PCA9685_MODE1 0x0
  6. #define PCA9685_PRESCALE 0xFE
  7. // Define first LED and all LED. We calculate the rest
  8. #define LED0_ON_L 0x6
  9. #define LEDALL_ON_L 0xFA
  10. #define PIN_ALL 16
  11. // Declare
  12. static void myPwmWrite(struct wiringPiNodeStruct *node, int pin, int value);
  13. static void myOnOffWrite(struct wiringPiNodeStruct *node, int pin, int value);
  14. static int myOffRead(struct wiringPiNodeStruct *node, int pin);
  15. static int myOnRead(struct wiringPiNodeStruct *node, int pin);
  16. int baseReg(int pin);
  17. /**
  18. * Setup a PCA9685 device with wiringPi.
  19. *
  20. * pinBase:       Use a pinBase > 64, eg. 300
  21. * i2cAddress:      The default address is 0x40
  22. * freq:            Frequency will be capped to range [40..1000] Hertz. Try 50 for servos
  23. */
  24. int pca9685Setup(const int pinBase, const int i2cAddress, float freq)
  25. {
  26.       // Create a node with 16 pins [0..15] + [16] for all
  27.       struct wiringPiNodeStruct *node = wiringPiNewNode(pinBase, PIN_ALL + 1);
  28.       // Check if pinBase is available
  29.       if (!node)
  30.             return -1;
  31.       // Check i2c address
  32.       int fd = wiringPiI2CSetup(i2cAddress);
  33.       if (fd < 0)             return fd;       // Setup the chip. Enable auto-increment of registers.       int settings = wiringPiI2CReadReg8(fd, PCA9685_MODE1) & 0x7F;       int autoInc = settings | 0x20;       wiringPiI2CWriteReg8(fd, PCA9685_MODE1, autoInc);              // Set frequency of PWM signals. Also ends sleep mode and starts PWM output.       if (freq > 0)
  34.             pca9685PWMFreq(fd, freq);
  35.       
  36.       node->fd                  = fd;
  37.       node->pwmWrite            = myPwmWrite;
  38.       node->digitalWrite      = myOnOffWrite;
  39.       node->digitalRead      = myOffRead;
  40.       node->analogRead      = myOnRead;
  41.       return fd;
  42. }
  43. /**
  44. * Sets the frequency of PWM signals.
  45. * Frequency will be capped to range [40..1000] Hertz. Try 50 for servos.
  46. */
  47. void pca9685PWMFreq(int fd, float freq)
  48. {
  49.       // Cap at min and max
  50.       freq = (freq > 1000 ? 1000 : (freq < 40 ? 40 : freq));       // To set pwm frequency we have to set the prescale register. The formula is:       // prescale = round(osc_clock / (4096 * frequency))) - 1 where osc_clock = 25 MHz       // Further info here: http://www.nxp.com/documents/data_sheet/PCA9685.pdf Page 24       int prescale = (int)(25000000.0f / (4096 * freq) - 0.5f);       // Get settings and calc bytes for the different states.       int settings = wiringPiI2CReadReg8(fd, PCA9685_MODE1) & 0x7F;      // Set restart bit to 0       int sleep      = settings | 0x10;                                                      // Set sleep bit to 1       int wake       = settings & 0xEF;                                                      // Set sleep bit to 0       int restart = wake | 0x80;                                                            // Set restart bit to 1       // Go to sleep, set prescale and wake up again.       wiringPiI2CWriteReg8(fd, PCA9685_MODE1, sleep);       wiringPiI2CWriteReg8(fd, PCA9685_PRESCALE, prescale);       wiringPiI2CWriteReg8(fd, PCA9685_MODE1, wake);       // Now wait a millisecond until oscillator finished stabilizing and restart PWM.       delay(1);       wiringPiI2CWriteReg8(fd, PCA9685_MODE1, restart); } /** * Set all leds back to default values (: fullOff = 1) */ void pca9685PWMReset(int fd) {       wiringPiI2CWriteReg16(fd, LEDALL_ON_L       , 0x0);       wiringPiI2CWriteReg16(fd, LEDALL_ON_L + 2, 0x1000); } /** * Write on and off ticks manually to a pin * (Deactivates any full-on and full-off) */ void pca9685PWMWrite(int fd, int pin, int on, int off) {       int reg = baseReg(pin);       // Write to on and off registers and mask the 12 lowest bits of data to overwrite full-on and off       wiringPiI2CWriteReg16(fd, reg       , on & 0x0FFF);       wiringPiI2CWriteReg16(fd, reg + 2, off & 0x0FFF); } /** * Reads both on and off registers as 16 bit of data * To get PWM: mask each value with 0xFFF * To get full-on or off bit: mask with 0x1000 * Note: ALL_LED pin will always return 0 */ void pca9685PWMRead(int fd, int pin, int *on, int *off) {       int reg = baseReg(pin);       if (on)             *on = wiringPiI2CReadReg16(fd, reg);       if (off)             *off = wiringPiI2CReadReg16(fd, reg + 2); } /** * Enables or deactivates full-on * tf = true: full-on * tf = false: according to PWM */ void pca9685FullOn(int fd, int pin, int tf) {       int reg = baseReg(pin) + 1;            // LEDX_ON_H       int state = wiringPiI2CReadReg8(fd, reg);       // Set bit 4 to 1 or 0 accordingly       state = tf ? (state | 0x10) : (state & 0xEF);       wiringPiI2CWriteReg8(fd, reg, state);       // For simplicity, we set full-off to 0 because it has priority over full-on       if (tf)             pca9685FullOff(fd, pin, 0); } /** * Enables or deactivates full-off * tf = true: full-off * tf = false: according to PWM or full-on */ void pca9685FullOff(int fd, int pin, int tf) {       int reg = baseReg(pin) + 3;            // LEDX_OFF_H       int state = wiringPiI2CReadReg8(fd, reg);       // Set bit 4 to 1 or 0 accordingly       state = tf ? (state | 0x10) : (state & 0xEF);       wiringPiI2CWriteReg8(fd, reg, state); } /** * Helper function to get to register */ int baseReg(int pin) {       return (pin >= PIN_ALL ? LEDALL_ON_L : LED0_ON_L + 4 * pin);
  51. }
  52. //------------------------------------------------------------------------------------------------------------------
  53. //
  54. //      WiringPi functions
  55. //
  56. //------------------------------------------------------------------------------------------------------------------
  57. /**
  58. * Simple PWM control which sets on-tick to 0 and off-tick to value.
  59. * If value is = 4096, full-on will be enabled
  60. * Every value in between enables PWM output
  61. */
  62. static void myPwmWrite(struct wiringPiNodeStruct *node, int pin, int value)
  63. {
  64.       int fd = node->fd;
  65.       int ipin = pin - node->pinBase;
  66.       if (value >= 4096)
  67.             pca9685FullOn(fd, ipin, 1);
  68.       else if (value > 0)
  69.             pca9685PWMWrite(fd, ipin, 0, value);      // (Deactivates full-on and off by itself)
  70.       else
  71.             pca9685FullOff(fd, ipin, 1);
  72. }
  73. /**
  74. * Simple full-on and full-off control
  75. * If value is 0, full-off will be enabled
  76. * If value is not 0, full-on will be enabled
  77. */
  78. static void myOnOffWrite(struct wiringPiNodeStruct *node, int pin, int value)
  79. {
  80.       int fd = node->fd;
  81.       int ipin = pin - node->pinBase;
  82.       if (value)
  83.             pca9685FullOn(fd, ipin, 1);
  84.       else
  85.             pca9685FullOff(fd, ipin, 1);
  86. }
  87. /**
  88. * Reads off registers as 16 bit of data
  89. * To get PWM: mask with 0xFFF
  90. * To get full-off bit: mask with 0x1000
  91. * Note: ALL_LED pin will always return 0
  92. */
  93. static int myOffRead(struct wiringPiNodeStruct *node, int pin)
  94. {
  95.       int fd = node->fd;
  96.       int ipin = pin - node->pinBase;
  97.       int off;
  98.       pca9685PWMRead(fd, ipin, 0, &off);
  99.       return off;
  100. }
  101. /**
  102. * Reads on registers as 16 bit of data
  103. * To get PWM: mask with 0xFFF
  104. * To get full-on bit: mask with 0x1000
  105. * Note: ALL_LED pin will always return 0
  106. */
  107. static int myOnRead(struct wiringPiNodeStruct *node, int pin)
  108. {
  109.       int fd = node->fd;
  110.       int ipin = pin - node->pinBase;
  111.       int on;
  112.       pca9685PWMRead(fd, ipin, &on, 0);
  113.       return on;
  114. }
pcs9685.h
 
  1. #ifdef __cplusplus
  2. extern "C" {
  3. #endif
  4. // Setup a pca9685 at the specific i2c address
  5. extern int pca9685Setup(const int pinBase, const int i2cAddress/* = 0x40*/, float freq/* = 50*/);
  6. // You now have access to the following wiringPi functions:
  7. //
  8. // void pwmWrite (int pin, int value)
  9. //            if value = 4096, set full-on
  10. //            else set PWM
  11. //
  12. // void digitalWrite (int pin, int value)
  13. //             if value != 0, set full-on
  14. //            else set full-off
  15. //
  16. // int digitalRead (int pin)
  17. //            read off-register
  18. //            To get PWM: mask with 0xFFF
  19. //            To get full-off bit: mask with 0x1000
  20. //            Note: ALL_LED pin will always return 0
  21. //
  22. // int analogRead (int pin)
  23. //            read on-register
  24. //            To get PWM: mask with 0xFFF
  25. //            To get full-on bit: mask with 0x1000
  26. //            Note: ALL_LED pin will always return 0
  27. // Advanced controls
  28. // You can use the file descriptor returned from the setup function to access the following features directly on each connected pca9685
  29. extern void pca9685PWMFreq(int fd, float freq);
  30. extern void pca9685PWMReset(int fd);
  31. extern void pca9685PWMWrite(int fd, int pin, int on, int off);
  32. extern void pca9685PWMRead(int fd, int pin, int *on, int *off);
  33. extern void pca9685FullOn(int fd, int pin, int tf);
  34. extern void pca9685FullOff(int fd, int pin, int tf);
  35. #ifdef __cplusplus
  36. }
  37. #endif

for python user

1) Install python-smbus and i2c-tools(if you have done, please skip)

 sudo  apt-get   install  -y   python-smbus   i2c-tools

2) To verify if I2C library is installed properly, you can run following terminal command:

sudo  i2cdetect  -y  1

or port 0 on the older Raspberry Pi

sudo  i2cdetect  -y  0

Once the program is running, the terminal will display a table like below showing PCA9685 address if any devices are connected, in this example the PCA9685 address is 0 x 40.

3)  Install PCA9685 python library:

cd  ~

git    clone  https://github.com/adafruit/Adafruit_python_PCA9685.git

cd  Adafruit_python_PCA9685

sudo  python  setup.py   install

4) Run program

cd  examples

sudo  python  ./simpletest.py

5) Running Result

Once the program is running, the servo motor will turn around between two angles.

Leave a Reply

WordPress spam blocked by CleanTalk.
Bombax Theme designed by itx
"