Problems with PCA9685 when using interrupts

kerem0comert · August 14, 2022, 12:05pm

Hello nice people of the internet,

I have been trying to create a quadruped spider robot. For this reason, I need to define movements such as move_left() or move_right().
If I just let the servos move to another angle at their own speeds, the legs do not synchronize and the robot just keeps sliding on the ground.
As such, I need several servos to move at the same time, for exactly a specified amount of time.
Using delay()'s for this purpose is not practical, as there could be a case where:
Joint 4 is commanded to move to position X in 3 seconds.
At 0.5 seconds, joint 4 receives another command which would override as the first command still needs 2.5 seconds to finish.

Using delays and busy waits interferes with the movement of the first command. As such, I am using interrupts.

Long story short, given the following code, the command line of Arduino produces expected results, but physically the servos just keep moving randomly.
So I suspect if the PCA9685 does not work so well with interrupts? Or am I missing something here?

I am using Arduino Uno , PCA9685 PWM Generator, SG90 Servo to make things work.

Looking forward to your responses.

Code:

//  --------                 --------
// |  D9    |               |  D7    |
// | joint9 |               | joint7 |
//  ----- --------     --------- -----
//       |  Foot 3 |  |  Foot4 |
//       |  D8     |  |  D6    |
//       | joint10 |  | joint6 |
//        --------     --------
//       |  Foot2 |  |  Foot1  |
//       |  D2    |  |   D4    |
//       | joint2 |  |  joint4 |
//  ----- --------    --------- -----
// |  D3    |               |  D5    |
// | joint3 |               | joint5 |
//  --------                 --------
//                Front

#define I2CAdd 0x40 // Define the I2C address of PCA9685
//#include <Servo.h>       //Call servo Library
#include "HCPCA9685.h" //Call PCA9685 library
#include <FlexiTimer2.h>
HCPCA9685 HCPCA9685(I2CAdd);

/*
  * Unsigned char is used rather than unsigned int for saving memory.
  * RELEVANT: Yes, interrupts occur, and are processed, during delay functions. 
  * When the ISR finishes, and returns, the code that was executing resumes executing 
  * where it left off. If a delay was in process, the delay will be resumed. 
  * The delay will end at the same time it would have if the interrupt had not occurred
*/

typedef struct Joint
{
  bool isMoving;
  unsigned long moveTimeSoFar;
  unsigned long targetTime;
  unsigned int currAngle;
  unsigned int targetAngle;
} Joint;

const unsigned char JOINT_COUNT = 8;
Joint joints[JOINT_COUNT];


void setup()
{
  Serial.begin(9600);
  // Initialize joints
  int defaultAngles[8] = {250, 40, 140, 350, 250, 40, 160, 350};
  int i;
  for (i = 0; i < JOINT_COUNT; i++)
  {
    joints[i].targetAngle = 0;
    joints[i].targetTime = 0;
    joints[i].isMoving = false;
    joints[i].moveTimeSoFar = 0;
    joints[i].currAngle = defaultAngles[i];
  }

  FlexiTimer2::set(20, checkMovements);
  FlexiTimer2::start();

  // Here, specify which move to start, e.g.:
  startTimedMovement(4, 150, 3000);
  startTimedMovement(5, 220, 3000);
}

void startTimedMovement(unsigned char jointId, unsigned int targetAngle, unsigned long targetTime)
{
  joints[jointId].targetTime = targetTime;
  joints[jointId].targetAngle = targetAngle;
  joints[jointId].isMoving = true;
  joints[jointId].moveTimeSoFar = 0;
}

void loop()
{
  // No need to do anything here, as the loop is handled by the checkMovements(void) interrupt
}
void wait_all_reach(void)
{
  for (int i = 0; i < JOINT_COUNT; i++)
  {
    while(1) if(joints[i].moveTimeSoFar > joints[i].targetTime) break;
  }
}

void checkMovements(void)
{
  sei();
  unsigned long currentTime = millis();
  int i;
  for (i = 0; i < JOINT_COUNT; i++)
  {
    if (joints[i].isMoving)
    {
      // Target time has been reached
      if (joints[i].moveTimeSoFar > joints[i].targetTime)
      {
        joints[i].targetAngle = 0;
        joints[i].targetTime = 0;
        joints[i].isMoving = false;
      }
      else
      { // Still need to move more
        joints[i].currAngle = map(
            joints[i].moveTimeSoFar, // Current progress
            0,                       // Start time
            joints[i].targetTime,    // End time
            joints[i].currAngle,
            joints[i].targetAngle);
        joints[i].moveTimeSoFar = currentTime;
        Serial.println("Servo " + String(i) + " moves to angle " + String(joints[i].currAngle) + ". MoveTimeSoFar " + String(joints[i].moveTimeSoFar));
        HCPCA9685.Servo(i, joints[i].currAngle);
      }
    }
  }
  Serial.println("It took function " + String(millis() - currentTime) + " seconds to complete");
}


**OUTPUT** (It is as expected as both servos move at the same time with each tick)

Servo 4 moves to angle 250. MoveTimeSoFar 48
15:03:49.643 -> Servo 5 moves to angle 40. MoveTimeSoFar 48
15:03:49.689 -> It took function 27 seconds to complete
15:03:49.783 -> Servo 4 moves to angle 249. MoveTimeSoFar 119
15:03:49.783 -> Servo 5 moves to angle 42. MoveTimeSoFar 119
15:03:49.877 -> It took function 96 seconds to complete
15:03:49.877 -> Servo 4 moves to angle 246. MoveTimeSoFar 258
15:03:49.923 -> Servo 5 moves to angle 49. MoveTimeSoFar 258
15:03:50.017 -> It took function 97 seconds to complete
15:03:50.017 -> Servo 4 moves to angle 238. MoveTimeSoFar 397
15:03:50.065 -> Servo 5 moves to angle 63. MoveTimeSoFar 397
15:03:50.112 -> It took function 97 seconds to complete
15:03:50.159 -> Servo 4 moves to angle 227. MoveTimeSoFar 537
15:03:50.251 -> Servo 5 moves to angle 83. MoveTimeSoFar 537
15:03:50.251 -> It took function 96 seconds to complete
15:03:50.299 -> Servo 4 moves to angle 214. MoveTimeSoFar 676
15:03:50.346 -> Servo 5 moves to angle 107. MoveTimeSoFar 676
15:03:50.394 -> It took function 98 seconds to complete
15:03:50.441 -> Servo 4 moves to angle 200. MoveTimeSoFar 816
15:03:50.489 -> Servo 5 moves to angle 132. MoveTimeSoFar 816
15:03:50.581 -> It took function 98 seconds to complete
15:03:50.581 -> Servo 4 moves to angle 187. Mov

mrburnette · August 14, 2022, 2:37pm

Delay(x) should never be utilized in any serious project.

There are several approaches, but I believe the better way is to review some similar responses:

robot leg arduino "synchronization" - Google Search

kerem0comert · August 14, 2022, 2:39pm

This is what I had said, delay() is not used in my implementation. As you can see I use interrupts and I get an expected output in the console, yet the movement of the servo is quite frantic and unexpected. So I am just wondering if I am missing some important detail with how servo and interrupts interact.

mrburnette · August 14, 2022, 6:33pm

Did you review the HPCA9684.h library file header?

/* If using an external clock change this value to match the clocks frequency 
   in Hz */
#define OSC_FREQ 25000000L

kerem0comert · August 23, 2022, 6:36pm

I set it to 16Mhz, since I have an Arduino Nano - the problem still persists.

jremington · August 23, 2022, 7:21pm

This is usually due to an inadequate servo power supply. How is your setup powered?

Successful projects budget at least 1 Ampere per servo, for small servos.

kerem0comert · August 28, 2022, 1:05pm

Thanks for the help, but I am using external power supply and I am sure that I have 1A/servo (I have 8A in total) .

I modified the code a bit, so currently this is what I have:

//  --------                 --------
// |  D9    |               |  D7    |
// | joint9 |               | joint7 |
//  ----- --------     --------- -----
//       |  Foot 3 |  |  Foot4 |
//       |  D8     |  |  D6    |
//       | joint10 |  | joint6 |
//        --------     --------
//       |  Foot2 |  |  Foot1  |
//       |  D2    |  |   D4    |
//       | joint2 |  |  joint4 |
//  ----- --------    --------- -----
// |  D3    |               |  D5    |
// | joint3 |               | joint5 |
//  --------                 --------
//                Front

#define I2CAdd 0x40 // Define the I2C address of PCA9685
//#include <Servo.h>       //Call servo Library
#include "HCPCA9685.h" //Call PCA9685 library
#include <FlexiTimer2.h>
HCPCA9685 HCPCA9685(I2CAdd);

/*
    Unsigned char is used rather than unsigned int for saving memory.
    RELEVANT: Yes, interrupts occur, and are processed, during delay functions.
    When the ISR finishes, and returns, the code that was executing resumes executing
    where it left off. If a delay was in process, the delay will be resumed.
    The delay will end at the same time it would have if the interrupt had not occurred
*/
typedef struct Vector3 {
  unsigned int x;
  unsigned int y;
  unsigned int z;
} Vector3;

typedef struct Joint
{
  bool isMoving;
  unsigned long moveTimeSoFar;
  unsigned long targetTime;
  unsigned int currAngle;
  unsigned int targetAngle;
} Joint;

enum MoveSpeeds {
  TURN = 4,
  LEG_MOVE = 8,
  BODY_MOVE = 3,
  STAND_SEAT = 1,
};

const unsigned char JOINT_COUNT = 8;
Joint joints[JOINT_COUNT];

const unsigned int SERVO_MOVEMENT_TIME = 2000;

void setup()
{
  Serial.begin(9600);
  // Initialize joints
  int defaultAngles[8] = {250, 40, 140, 350, 250, 40, 160, 350};
  int i;
  for (i = 0; i < JOINT_COUNT; i++)
  {
    joints[i].targetAngle = 0;
    joints[i].targetTime = 0;
    joints[i].isMoving = false;
    joints[i].moveTimeSoFar = 0;
    joints[i].currAngle = defaultAngles[i];
  }


  // Here, specify which move to start, e.g.:
  startTimedMovement(1, 150, 3000);
  startTimedMovement(2, 220, 3000);
  startTimedMovement(3, 220, 3000);
  startTimedMovement(4, 220, 3000);
  startTimedMovement(5, 220, 3000);
  //forward(5);

  FlexiTimer2::set(200, checkMovements);
  FlexiTimer2::start();

}

void startTimedMovement(unsigned char jointId, unsigned int targetAngle, unsigned long targetTime)
{
  joints[jointId].targetTime = targetTime;
  joints[jointId].targetAngle = targetAngle;
  joints[jointId].isMoving = true;
  joints[jointId].moveTimeSoFar = 0;
}

void loop()
{
  //Serial.println("test");
}


/*void wait_all_reach(void)
  {
  for (int i = 0; i < JOINT_COUNT; i++)
  {
    while(1) if(joints[i].moveTimeSoFar > joints[i].targetTime) break;
  }
  }
*/
void checkMovements(void)
{
  sei();
  unsigned long currentTime = millis();
 // Serial.println(currentTime);
  int i;
  for (i = 0; i < JOINT_COUNT; i++)
  {
    if (joints[i].isMoving)
    {
      // Target time has been reached
      if (joints[i].moveTimeSoFar > joints[i].targetTime)
      {
        joints[i].targetAngle = 0;
        joints[i].targetTime = 0;
        joints[i].isMoving = false;
      }
      else
      { // Still need to move more
        joints[i].currAngle = map(
                                joints[i].moveTimeSoFar, // Current progress
                                0,                       // Start time
                                joints[i].targetTime,    // End time
                                joints[i].currAngle,
                                joints[i].targetAngle);
        joints[i].moveTimeSoFar = currentTime;
        Serial.println("Servo " + String(i) + " moves to angle " + String(joints[i].currAngle) + ". MoveTimeSoFar " + String(joints[i].moveTimeSoFar));
       HCPCA9685.Servo(i + 2, joints[i].currAngle);
        //HCPCA9685.Servo( 2, 250);
      }
    }
  }
  //Serial.println("It took function " + String(millis() - currentTime) + " seconds to complete");
  cli();
}


void forward(unsigned short int step) {
  while (step-- > 0) {

    startTimedMovement(4, joints[4].currAngle - 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(6, joints[6].currAngle - 50, SERVO_MOVEMENT_TIME);
    delay(100);

    startTimedMovement(0, joints[0].currAngle - 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(4, joints[4].currAngle + 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(2, joints[2].currAngle - 60, SERVO_MOVEMENT_TIME);
    startTimedMovement(6, joints[6].currAngle + 60, SERVO_MOVEMENT_TIME);

    delay(100);

    startTimedMovement(3, joints[3].currAngle + 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(7, joints[7].currAngle + 50, SERVO_MOVEMENT_TIME);

    delay(100);

    startTimedMovement(1, joints[1].currAngle + 60, SERVO_MOVEMENT_TIME);
    startTimedMovement(5, joints[5].currAngle + 60, SERVO_MOVEMENT_TIME);

    delay(100);

    startTimedMovement(0, joints[0].currAngle + 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(4, joints[4].currAngle - 50, SERVO_MOVEMENT_TIME);
    startTimedMovement(2, joints[2].currAngle + 60, SERVO_MOVEMENT_TIME);
    startTimedMovement(6, joints[6].currAngle - 60, SERVO_MOVEMENT_TIME);
    delay(100);

    startTimedMovement(1, joints[1].currAngle - 60, SERVO_MOVEMENT_TIME);
    startTimedMovement(5, joints[5].currAngle - 60, SERVO_MOVEMENT_TIME);
    delay(100);

  }
}

Literally each time I run the code without changing a line, I get a different serial output. It is always the case that the code stops working—but in different moments in time at each run.

Some of the outputs I get with this exact same code:

Servo 1 moves to angle 40. MoveTimeSoFar 198
Servo 2 moves to angle 140. Mov

Se

Servo 1 moves to angle 40. MoveTimeSoFar 198
Servo 2 moves to angle 140. MoveTimeSoFar 198
Servo 3 moves to angle 350. Mov

Is this a timing issue? Or is it the case that I do not have enough RAM etc? Looking forward for feedback.

system · February 24, 2023, 1:05pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Speed controlling of servos using PCA9685 Programming	10	865	June 18, 2023
PCA9685 works with LEDs but not with Servos General Guidance	15	2707	August 20, 2022
PCA9685 Intermittently Stops Responding Robotics	4	47	October 2, 2024
Moving Servos with the PCA9685 Board General Electronics	11	76	January 15, 2025
PCA9685 with Millis Programming	22	142	March 11, 2025

Problems with PCA9685 when using interrupts

Related topics