Using SPI Interrupt causes two servos to lose functionality

Question

My Arduino MEGA clone (ATMEGA 2650) reads SPI data as a slave using an interrupt.
There are also two servos that make an arm (shoulder and elbow).

• When I comment out the line that attaches the SPI interrupt, the servos work properly.

• When the SPI interrupt is attached, three strange things happen:

  1) The PWM of the elbow remains at a fixed duty cycle. The shoulder works fine.

  2) The shoulder PWM is somewhat jittery time-wise, I assume though that this is unavoidable with interrupts.

  3) Writing an angle to either servo, (appears to) write to both servos! (In the software anyway.) So if I only write to the elbow, the shoulder will have the same value ( printed with shoulder.read() ). And vise-versa.

Again, none of those problems occur when I comment out the SPI interrupt attach.

It doesn't make sense to me though. Why would attaching an interrupt make two servos have the same value? And how can the values of the servos be the same, when only one PWM duty cycle actually changes!?

I wonder if this is a timer issue. Maybe the SPI interrupt interferes with the servo timers or something.

How can I receive SPI messages while also controlling two servos?

#include <Servo.h>
#include <SPI.h>

const float pi = 3.14159;
const float L1 = 27;
const float L2 = 27;
float c2;
float s2;
float psi;
float theta;

float angle_shoulder = 90;
float angle_elbow = 90;

volatile int arm_x;
volatile int arm_y;
volatile int puck_x;
volatile int puck_y;

Servo shoulder;
Servo elbow;

int sweepDir = 0;
int sweep1 = 0;

unsigned char spiBuffer[5];
volatile byte buffPos;
volatile bool process_spiBuffer;


void setup()
{
    Serial.begin(115200); //Initialize serial communication
    Serial.println("### SETUP ###");

    shoulder.attach(25); //5
    elbow.attach(43); //3

    // BEGIN SPI SETUP 
    // Turn on SPI in slave mode
    SPCR |= bit(SPE);

    // Send on master in, *slave out*
    pinMode(MISO, OUTPUT);

    // Get ready for an interrupt
    buffPos = 0;   // buffer empty
    process_spiBuffer = false;

    // Turn on interrupts
    SPI.attachInterrupt();

    // END SPI SETUP
} // END SETUP()


void loop()
{
    arm_y = 30;

    if (process_spiBuffer)
    {
        spiBuffer[buffPos] = 0;
        // X is a two-byte value, SPI only sends one byte at a time, so merge the bits upon receipt
        puck_x = (spiBuffer[1] << 8) | spiBuffer[2];
        puck_y = spiBuffer[3];
        //Check if received message is acceptable, else throw warning and reject message
        if (spiBuffer[0] != '<' || spiBuffer[4] != '>' || puck_x < 0 || puck_x > 320 || puck_y < 0 || puck_y > 240)
        {   //Message received was corrupted
            Serial.print("SPI: Bad MSG Data!:\"");
            //Serial.print(spiBuffer);
        }
        else
        {   //Message received "OK" 

            Serial.print("SPI MSG Received:\"");
            //Serial.print(spiBuffer);
            Serial.println("\"");

            Serial.print("SPI puck coords:(");
            Serial.print(puck_x);
            Serial.print(",");
            Serial.print(puck_y);
            Serial.println(")");

            // Decide on new paddle position. (x,y) of puck is 90degrees CW from AI (arm) POV, so x and y are swapped. Arm origin (0,0) is at bottom-right corner from the AI POV.
            arm_x = map((240 - puck_y), 1, 320, 5, 49); // map(value, fromLow, fromHigh, toLow, toHigh)

        }
        buffPos = 0;
        process_spiBuffer = false;
    } 

    crunchAngles();
    moveArm();

} // END loop()

void crunchAngles()
{
    c2 = (sq(arm_x) + sq(arm_y) - sq(L1) - sq(L2)) / (2 * L1 * L2);
    s2 = sqrt(1 - sq(c2));

    angle_elbow = acos((sq(arm_x) + sq(arm_y) - sq(L1) - sq(L2)) / (2 * L1 * L2)) * (180 / pi); // theta?
    angle_shoulder = asin((arm_y * (L1 + L2 * c2) - arm_x * L2 * s2) / (sq(arm_x) + sq(arm_y))) * (180 / pi); // psi?

}

// Update the servos
void moveArm()
{
    // TODO: Clip servo output with serial warning
    if (angle_shoulder < -10) { angle_shoulder = -10; } // Actual min angle is about -18deg (Model HS-225BB)
    else if (angle_shoulder > 179) { angle_shoulder = 179; } // Actual max angle is about 181deg (Model HS-225BB)

    shoulder.write(180 - angle_shoulder + 2); // Shoulder increments clockwise, so subtract from 180

    if (angle_elbow < 0) { angle_elbow = 0; } // Actual max angle is about -5deg (Model SG90)
    else if (angle_elbow > 170) { angle_elbow = 170; } // Actual max angle is about 190deg (Model SG90)

    elbow.write(angle_elbow + 3); // Elbow increments clowise from 

}

ISR(SPI_STC_vect)
{
    byte c = SPDR;  

    if (buffPos < sizeof spiBuffer)
    {
        spiBuffer[buffPos++] = c;

        // End of message is ">" 
        if (c == '>') // if (c == '\n') 
            process_spiBuffer = true;

    }  
}

Answer 1

You have multiple issues here. For a start, you have a buffer (spiBuffer) of 5 bytes, into which you place <Xxy> (that's 5 bytes) and then finish off with:

    spiBuffer[buffPos] = 0;

That has now overwritten some other memory.

---

Next, you don't seem to be detecting the "<" symbol and resetting to the start of the buffer, so if you ever get out of sync, you will stay out of sync. I suggest something like making buffPos equal to 0xFF initially. Then in the ISR:

ISR(SPI_STC_vect)
{
    byte c = SPDR;  

    // exit if haven't processed last one
    if (process_spiBuffer)
      return;

    if (c == '<')
      buffPos = 0;
    else if (buffPos == 0xFF)
      return;  // ignore until we get a '<'

    if (buffPos < sizeof spiBuffer)
    {
        spiBuffer[buffPos++] = c;

        // End of message is ">" 
        if (c == '>') 
            process_spiBuffer = true;

    }  
}

Now we use 0xFF as a "flag" that we haven't received a "<" yet. When we do, we reset buffPos to zero, ready to process the next 5 bytes. In the main loop, once we are done with processing the buffer we set buffPos to 0xFF again (rather than zero). Also you might have the ISR called before you have processed the previous one, so I put in a test to exit if process_spiBuffer is true when you enter the ISR.

---

I found your code to work out the arm positions confusing. For debugging this issue I would replace it with simply moving the servo to the position received by SPI, eg.

   // crunchAngles();
   // moveArm();

   elbow.write (puck_x);

Now just debug that rather than getting confused about the difference between interrupt issues and logic issues. I found once I had done that, that my servo responded without any major issues (there was a bit of jittering occasionally).

---

I found that I got a lot of data errors until I slowed down the sender (a few microseconds delay after each SPI.transfer) because sending SPI at high speed didn't give the receiving sketch time to put the data into the buffer.

---

The servo library is rather dependent on interrupts (it manually does the PWM duty cycles). I wrote a sketch a while back that controls one server by using the hardware timer (Timer 1) rather than interrupts:

const byte potpin = A0;  // analog pin used to connect the potentiometer

const unsigned long PRESCALER = 8;         // Timer 1 prescaler
const float PULSE_PERIOD = 0.020;          // 20 ms
const float ZERO_POSITION_WIDTH = 0.0005;  // 0.5 ms
const float FULL_POSITION_WIDTH = 0.0024;  // 2.4 ms

// how far apart the pulses are
const unsigned long PULSE_WIDTH_COUNT = F_CPU / PRESCALER * PULSE_PERIOD;
// minimum pulse width (-45 degrees)
const unsigned long ZERO_POSITION_COUNT = F_CPU / PRESCALER * ZERO_POSITION_WIDTH;
// minimum pulse width (+45 degrees)
const unsigned long FULL_POSITION_COUNT = F_CPU / PRESCALER * FULL_POSITION_WIDTH;

void setup()
  {
  TCCR1A = 0;          // disable all PWM on Timer1 whilst we set it up
  ICR1 = PULSE_WIDTH_COUNT - 1;   // frequency is every 20ms (zero-relative)

  // Configure timer 1 for Fast PWM mode using ICR1, with 8x  prescaling
  TCCR1A = bit (WGM11);
  TCCR1B = bit (WGM13) | bit (WGM12) |  bit (CS11);  // fast PWM top at ICR1
  TCCR1A |= bit (COM1A1);  // Clear OC1A/OC1B on Compare Match,
  pinMode (9, OUTPUT);
  }   // end of setup

void loop()
{
  int val = analogRead(potpin);  // reads the value of the potentiometer (value between 0 and 1023)
  OCR1A = ZERO_POSITION_COUNT + (val * (FULL_POSITION_COUNT - ZERO_POSITION_COUNT) / 1024) - 1;
  delay(15);                     // wait for the servo to get there
} // end of loop

That is for the Atmega328P, not the Mega, but you could adapt it easily enough. You could use Timer 2 for the other servo (I think).

Since it doesn't use interrupts, then having interrupts going off in the background won't affect the servo.

---

Example code

After considerable mucking around, I've got a simpler case working reliably.

Writing an angle to either servo, (appears to) write to both servos! (In the software anyway.) So if I only write to the elbow, the shoulder will have the same value ( printed with shoulder.read()

That's exactly what happened to me! Until ... I realized I should power the servos independently. As soon as I connected both servo's +5V and Gnd to an external power supply (and connected the ground of the power supply to the Mega) it all started working properly. So, lesson #1 is: power your motors properly!

I made up a test "driver" on my Uno using the code below:

#include <SPI.h>

void setup (void)
{
  digitalWrite(SS, HIGH);  // ensure SS stays high
  SPI.begin ();
  SPI.setClockDivider(SPI_CLOCK_DIV32);
}


void loop (void)
{
  int val;

  // enable Slave Select
  digitalWrite(SS, LOW);

  SPI.transfer ('<');
  val = analogRead (0);    // read A0
  SPI.transfer (highByte(val));
  SPI.transfer (lowByte (val));
  val = analogRead (1);    // read A1
  SPI.transfer (highByte(val));
  SPI.transfer (lowByte (val));
  SPI.transfer ('>');

  // disable Slave Select
  digitalWrite(SS, HIGH);

  delay (100);
}

That reads a pot on A0 and A1 and sends the full value (2 bytes) over SPI (which is slightly different to what you are doing).

Then the receiving end (on a Mega2560) has this code:

#include <SPI.h>

const unsigned long PRESCALER = 8;         // Timer prescaler
const float PULSE_PERIOD = 0.020;          // 20 ms
const float ZERO_POSITION_WIDTH = 0.0005;  // 0.5 ms
const float FULL_POSITION_WIDTH = 0.0024;  // 2.4 ms

// how far apart the pulses are
const unsigned long PULSE_WIDTH_COUNT = F_CPU / PRESCALER * PULSE_PERIOD;
// minimum pulse width (-45 degrees)
const unsigned long ZERO_POSITION_COUNT = F_CPU / PRESCALER * ZERO_POSITION_WIDTH;
// minimum pulse width (+45 degrees)
const unsigned long FULL_POSITION_COUNT = F_CPU / PRESCALER * FULL_POSITION_WIDTH;

const byte NO_DATA_YET = 0xFF;

volatile unsigned char spiBuffer[6];  // '<' xH xL yH yL '>'
volatile byte buffPos = NO_DATA_YET;
volatile bool process_spiBuffer = false;

void setup()
  {
  Serial.begin (115200);   // debugging

  // Turn on SPI in slave mode
  SPCR |= bit(SPE);

  // Send on master in, *slave out*
  pinMode(MISO, OUTPUT);

  // Turn on interrupts
  SPI.attachInterrupt();

  TCCR1A = 0;          // disable all PWM on Timer1 whilst we set it up
  ICR1 = PULSE_WIDTH_COUNT - 1;   // frequency is every 20ms (zero-relative)

  // Configure timer 1 for Fast PWM mode using ICR1, with 8x prescaling
  TCCR1A = bit (WGM11);
  TCCR1B = bit (WGM13) | bit (WGM12) |  bit (CS11);  // fast PWM top at ICR1
  TCCR1A |= bit (COM1A1);  // Clear OC1A/OC1B on Compare Match,

  #ifdef __AVR_ATmega2560__

    TCCR3A = 0;          // disable all PWM on Timer3 whilst we set it up
    ICR3 = PULSE_WIDTH_COUNT - 1;   // frequency is every 20ms (zero-relative)

    // Configure timer 3 for Fast PWM mode using ICR3, with 8x prescaling
    TCCR3A = bit (WGM31);
    TCCR3B = bit (WGM33) | bit (WGM32) |  bit (CS31);  // fast PWM top at ICR3
    TCCR3A |= bit (COM3A1);  // Clear OC1A/OC1B on Compare Match,

    pinMode (11, OUTPUT);   // OC1A is D11 on Mega2560
    pinMode (5, OUTPUT);    // OC3A is D5  on Mega2560
  #else
    pinMode (9, OUTPUT);    // OC1A is D9  on Uno
  #endif
  }   // end of setup

int xValue,
    yValue;

void loop()
{

  if (process_spiBuffer)
    {
    if (spiBuffer [0] == '<' && spiBuffer [5] == '>')
      {
      xValue = makeWord (spiBuffer[1], spiBuffer[2]);
      yValue = makeWord (spiBuffer[3], spiBuffer[4]);
      }

    // ready for another interrupt
    buffPos = NO_DATA_YET;
    process_spiBuffer = false;

    }

  // adjust PWM amount
  OCR1A = ZERO_POSITION_COUNT + (xValue * (FULL_POSITION_COUNT - ZERO_POSITION_COUNT) / 1024) - 1;

  #ifdef __AVR_ATmega2560__
    OCR3A = ZERO_POSITION_COUNT + (yValue * (FULL_POSITION_COUNT - ZERO_POSITION_COUNT) / 1024) - 1;
  #endif

} // end of loop


ISR(SPI_STC_vect)
{
  byte c = SPDR;   // get byte from SPI hardware

  // exit if haven't processed last one
  if (process_spiBuffer)
    return;

  if (c == '<')
    buffPos = 0;
  else if (buffPos == NO_DATA_YET)
    return;  // ignore until we get a '<'

  if (buffPos < sizeof spiBuffer)
    {
    spiBuffer[buffPos++] = c;

    // End of message is ">" 
    if (c == '>') 
      process_spiBuffer = true;

   }  // end of room in buffer
} // end of ISR(SPI_STC_vect)

This incorporates my suggested use of the timer hardware to do the servo positioning, rather than the servo library. There are a couple of #ifdefs in there, so you can run it on a Uno as well (it just won't do the second servo).