A4988 Stepper control via Timer unstable when using Serial Input

I am currently using an Arduino Nano to control Stepper Motors (accelaration control) via A4988 Drivers. To accomplish that, I use the timer1. I want to adjust values via bluetooth. As soon as I start sending data via bluetooth (even if I send a zero, which doesnt influence the dynamics), the steppers act weirdly and the system becomes unstable (it’s a self balancing robot). Any ideas why? Here’s the code. I’ve shortened it, to stay well under the maximum character count per post.

#include "I2Cdev.h"

const unsigned long READ_PERIOD = 20;
const unsigned long SEND_PERIOD = 200;
long lastTime = 0;
long lastSendTime = 0;

void setup() {
  //8 microsteps
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(9600);
  Serial.println("Initializing I2C devices...");

  accelgyro.initialize();
  accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);  //5 Hz DLPF
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);   // Accel Range of +- 2g

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  //Set the timer to interrupt the program with the Frequency of interruptFreq (50kHz). This enables Pulses of 50kHz or slower which equals a maximum RPM of about 900
  //Once the timer triggers, ISR(TIMER2_COMPA_vect) gets called
  /* TCCR2A = 0;                                                               //Make sure that the TCCR2A register is set to zero
    TCCR2B = 0;                                                               //Make sure that the TCCR2A register is set to zero
    TIMSK2 |= (1 << OCIE2A);                                                  //Set the interupt enable bit OCIE2A in the TIMSK2 register
    TCCR2B |= (1 << CS21);                                                    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
    OCR2A = compare;                                                          //The compare register is set to float compare
    TCCR2A |= (1 << WGM21);     */                                              //Set counter 2 to CTC (clear timer on compare) mode
  // STEPPER MOTORS INITIALIZATION
  // TIMER1 CTC MODE
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |=  (1 << WGM12);
  TCCR1A &= ~(1 << WGM11);
  TCCR1A &= ~(1 << WGM10);

  // output mode = 00 (disconnected)
  TCCR1A &= ~(3 << COM1A0);
  TCCR1A &= ~(3 << COM1B0);

  // Set the timer pre-scaler
  // Generally we use a divider of 8, resulting in a 2MHz timer on 16MHz CPU
  TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);

  //OCR1A = 125;  // 16Khz
  //OCR1A = 100;  // 20Khz
  OCR1A = compare;   // 25Khz
  TCNT1 = 0;
  TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  kalmanX.setAngle(asin(maptoG(ay) / 9810) * 180 / PI);
  KalmanAngleLast = 0;
  Serial.println("Setup complete");
  timer = micros();
}

void loop() {
  //ensure stability running with a fixed samplerate of 20ms
  if (millis() - lastTime >= READ_PERIOD) {
    calcRPM = calcRPM + calcACC * READ_PERIOD / 1000 + 1;    lastTime += READ_PERIOD;
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Hier: Drehung um X -> ay, gx interessant
    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
    float angle = atan2(maptoG(ay), maptoG(az)) * 180 / PI;
    KalmanAngle = kalmanX.getAngle(angle, gx / 131.0, dt);
    if (abs(KalmanAngle) > 37.5) {
      emergency = true;
    }
    arel =  (KalmanAngle - KalmanAngleLast) / dt;
    int deadzone = 2;
    // RPM/s
    calcACC = (P * KalmanAngle + D * (KalmanAngle - KalmanAngleLast) / dt);
    KalmanAngleLast = KalmanAngle;

    // AdjustSpeed via following formula: pulseFreq = RPM/60*200*Microsteps;
    pulseFreq = (-calcRPM / 60) * 200 * Microsteps ;
    if (abs(KalmanAngle) < 0 || emergency) {
      SpeedToTimerCount = 0;
    }
    else {
      if (pulseFreq != 0) {
        SpeedToTimerCountTemp = interruptFreq / (2 * pulseFreq);
      }
      if ((abs(SpeedToTimerCount) == 1000 && abs(SpeedToTimerCountTemp) < 950) || SpeedToTimerCount < 1000) {
        //Calculate how many Interrupts are needed until the desired Pulse Length is reached

        if (SpeedToTimerCountTemp < 5 && SpeedToTimerCountTemp >= 0) {
          SpeedToTimerCount = 5;
        }
        else if (SpeedToTimerCountTemp > -5 && SpeedToTimerCountTemp <= 0) {
          SpeedToTimerCount = -5;
        }
        else if (abs(SpeedToTimerCountTemp) > 1000) {
          if (SpeedToTimerCountTemp > 0) {
            SpeedToTimerCount = 1000;
          }
          else if (SpeedToTimerCountTemp < 0) {
            SpeedToTimerCount = -1000;
          }
        }
        else {
          SpeedToTimerCount = SpeedToTimerCountTemp ;
        }

      }
    }
    if (sample_count < NUM_SAMPLES) {
      sum += analogRead(A2);
      sample_count++;
    }
    else {
      voltage = ((float)sum / (float)NUM_SAMPLES * 5.0) / 1024.0;
      sample_count = 0;
      sum = 0;
    }
    recvWithStartEndMarkers();
    if (newData == true) {
      VRead = atoi(receivedChars);
      Serial.print("VRead");
      Serial.print(";");
      Serial.println(KalmanAngle);
      newData = false;
    }

  }
}
float mapfloat(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

ISR(TIMER1_COMPA_vect) {
  counter++;
  if (counter >= abs(SpeedToTimerCountMemory)) {
    SpeedToTimerCountMemory = SpeedToTimerCount;
    counter = 0;
    if (SpeedToTimerCountMemory < 0) {
      PORTB &= 0b11111101;
      PORTD |= 0b00100000;
    }
    else {
      PORTB |= 0b00000010;
      PORTD &= 0b11011111;
    }
  }
  else if (counter == 1) {
    PORTB |= 0b00000100;
    PORTD |= 0b01000000;

  }
  else if (counter == 2) {
    PORTB &= 0b11111011;
    PORTD &= 0b10111111;
  }

}
int maptoG (int16_t value) {
  int returnV = map(value, -32768, +32767, -2 * 9810, +2 * 9810);
  return returnV;
}

void recvWithStartEndMarkers() {

  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;

  if (Serial.available() > 0 && newData == false) {
    while (Serial.available() > 15 &&  recvInProgress == false) {
      for (int i = 0; i < 7; i++) {
        Serial.read();
      }
    }
    rc = Serial.read();

    if (recvInProgress == true) {
      if (rc != endMarker) {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;
        }
      }
      else {
        receivedChars[ndx] = '\0'; // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker) {
      recvInProgress = true;
    }
  }
}

I greatly appreciate every help!

ACCSelfBalancingRobotInterruptTimerComplete.ino (8.16 KB)

As soon as I start sending data via bluetooth (even if I send a zero, which doesnt influence the dynamics), the steppers act weirdly and the system becomes unstable (it’s a self balancing robot). Any ideas why?

My guess is that the interrupts used by the bluetooth serial are delaying the execution of the Timer1 interrupts.

How is the bluetooth connected? Are you using Software serial? Hardware serial?

If you disconnect the bluetooth and send you commands over the serial monitor at 115200 baud, do you still see the effect?

What is this all about?

while (Serial.available() > 15 &&  recvInProgress == false) {
      for (int i = 0; i < 7; i++) {
        Serial.read();
      }

Bluetooth is connected via Tx Rx pin, so it's hardwareSerial.

I'm trying to prevent input buffer overflow with the while loop, as it just reads all the bytes if there's more than two messages in the buffer. Not sure if I need it though. It is hard to judge, if the problem persists with the serial monitor as I can't use the motors in that case (the system will either move, or, if I hold in the air, behave weirdly because the control loop won't work).

MJLennox: I am currently using an Arduino Nano to control Stepper Motors (accelaration control) via A4988 Drivers. To accomplish that, I use the timer1.

How many steps per second are you generating?

How many bytes are there in a Bluetooth message?

...R

Robin2: How many steps per second are you generating?

Really depends on the situation. As it is a control loop, the steps/second are generated automatically. However the highest I CAN go (I set a limit) is one microstep every 50 microseconds. The problem also occcurs on much lower step frequencies though (e.g. 1 step every 2000micros).

Robin2: How many bytes are there in a Bluetooth message?

The bytes per message differ, I've tried with as low as about 4 bytes, but I plan to use more later.

There is a lot going on. I can not pinpoint the problem, but I do see a number of problems.

The Arduino Nano is a 5V Arduino board and the MPU-6050 is a 3.3V sensor. I hope you have level shifters for SDA and SCL.

The Bluetooth module needs current. Perhaps your power is not enough. How do you power the Nano board ? Is the 5V pin near 5.0V ? Is there electrical noise between the Bluetooth module, the Arduino and the motors ? Not having enough power or electrical noise would be the first thing that I would check. The second thing is the Bluetooth module. If it has some kind of echo, and it returns the data, then everything will go wrong.

The Bluetooth is connected to the Arduino Nano with pin 0 and 1 ? So if you send something to the serial monitor, the Bluetooth module will also receive it. All those messages are send to the Bluetooth module ? How can you upload a sketch if the RX pin is blocked by the Bluetooth module ?

The I2Cdev library uses processing time. With 100Hz there is not much left. Are you setting the sensor to 5Hz and checking it with 50Hz ?

You have shortened your sketch, but now I can not find the declaration of the variable 'counter', 'newData', the 'SpeedToTimer...' variables and the 'receivedChars' array. Some should be 'volatile' and I can not check the bounderies of the array. There is a website for that: https://snippets-r-us.com/

The I2Cdev library has bugs: https://github.com/jrowberg/i2cdevlib/issues/265. There is no such thing as a "normal" write and a "fast" write. That is a myth.

Reading the data from the sensor and reading data from the serial port are two seperate tasks. Therefor the function 'recvWithStartEndMarkers()' should be at the main level in the loop(), not behind the millis-timer of the sensor.

You fill up the 'receivedChars[]' with received data. I'm not sure if your code makes a zero-terminated string of that. It is easier if you always have a zero-terminator at the end, so you can send it to the serial monitor at any time for debugging.

When dealing with millis(), always use 'unsigned long'. Your 'lastTime' and 'lastSendTime' are 'long'.

With so many problems and bugs, I wonder if the actual code and the usage of the hardware timers is okay.

The MPU-6050 is noisy. The MPU-9250 is better. The I2Cdev library has bugs and is outdated. The "Kalman" filter in the I2Cdev library has many critics, some say that it is not a Kalman filter. There is a AHRS filter by kriswiner which is used as the base for other libraries: https://github.com/kriswiner/MPU9250. When using the MPU-9250 or newer version, you could upgrade to a 3.3V Arduino board. For example one of the MKR boards. Then you could also use the SPI mode for the sensor.

First of all: the code works perfectly without Bluetooth input! I have a 5v 3 amps power supply and the motors get supplied externally.

I unplug the bluetooth module before uploading code. When I use the Serial Monitor for debuggin I use other outputs. Right now Serial.print is exactly what I want to be delivered via bluetooth (and it does exactly that.).

I will put recvWithStartEndMarkes() into the main, agreed. I haven't spend too much time bothering with this function as I copied it from Serial Input Basics by Robin2. Will change the data types to unsigned long.

As said in the beginning, I am quite happy with how the code works so far, aside from the problem described. I understand that there are other things that can be improved such as using a better MPU etc. but I doubt that that's the cause of the problem. Thanks for helping though.

Will test with the Code improvements you told me about and then come back to you, with the full code.

Changing that helped a bit, but not really. The problem still occurs. I think what was suggested in #1 sounds reasonable. How can I find out? I attached the full code to the Original Post.

The thing you said about volatile variables sounds reasonable, too, but I have no clue about it..

bump

It does not work perfectly :'( When you measure the data on the I2C bus, you will see that it is a big mess because of the bugs in the I2Cdev lib.

Does this mean that you have the SDA and SCL of the MPU-6050 directly connected to the SDA and SCL of the Arduino Nano ? That is not okay, and since you have a problem you better fix also these minor problems.

When everything is okay, then I think that interrupts do not get tangled up too much. It is just a feeling.

Connecting the Bluetooth module to pin 0 and 1 and still using the serial monitor and uploading a sketch is a big mystery for me. It can't be right. You are sending a lot of messages to the Bluetooth module without reason. I do not know how the Bluetoot module deals with that.

cattledog wrote about disconnecting the Bluetooth module and using the serial monitor. You can then type the commands in the serial monitor.

When you send a lot of text to the serial monitor at 9600 baud, then the TX-buffer in the Serial library could get full. When it is full, the speed of the sketch will drop drastic. Using 115200 as cattledog wrote will solve that.

The compiler might keep a variable in registers (the Arduino Nano has 32 registers) and not write the new data to its memory location. When you use a variable both in a interrupt and in the loop(), then you must make that variable 'volatile'. The compiler will then write new data to the memory location of that variable.

When you use a 16-bit variable on a 8-bit microcontroller and use that variable in a interrupt and in the loop(), then it is possible that the variable is read or written only half and a interrupt will change that value. You better use 8-bit variables or turn of the interrupts off in the loop() when reading and writing that variable.

MJLennox: The problem also occcurs on much lower step frequencies though (e.g. 1 step every 2000micros).

Then there is something seriously wrong. There should be no trouble reading Serial data if other things happen once every 2 millisecs.

Sorry, but I'm too lazy to take the time that I would need to understand code that uses the Timer registers to a sufficient extent to recognize where a problem might lie.

The bytes per message differ, I've tried with as low as about 4 bytes, but I plan to use more later.

It is the max that you plan to use that matters. You should be testing that at an early stage.

...R

Koepel: It does not work perfectly :'( When you measure the data on the I2C bus, you will see that it is a big mess because of the bugs in the I2Cdev lib.

When I first tested the Hardware I debugged the Angle I got out of the Gy-521 (with the MPU6050 onboard) and that seemed fine. at least that's what I can see when printing the angle. I also wanna emphasize that the systems did INDEED work perfectly fine, apart from the bluetooth part. It can balance itself perfectly when I am not receiving any bluetooth data on the arduino.

Koepel: Does this mean that you have the SDA and SCL of the MPU-6050 directly connected to the SDA and SCL of the Arduino Nano ? That is not okay, and since you have a problem you better fix also these minor problems.

Never have I said that I used all of this simultanously. When I upload a sketch, I first unplug the HC-05 from Pin0 and 1. Also I never have the USB (or serial monitor) connected to the Arduino, when using the HC-05. I don't know what made you think that. The Serial.Write commands are for sending Data via bluetooth, not to print them to the Serial monitor.

I have changed the Baudrate to said value (and also adjusted it on the bluetooth module) and made the variables volatile. That didn't help either.

I did some testing without the HC-05 and used the Serial Monitor instead. I spammed as quickly as possible and couldn't get the arduino to freeze..

I also now tried different devices/software to send the data TO the HC-05 (the data, the arduino receives). Still causes the issue. I am starting to think it's something that's related to the arduino and HC-05. Any idea?

Thanks, that makes sense. I am slowly getting a grip on understanding your project and your problem :wink:

Do you have the FASTWIRE enabled ? Then I can check the library if that does something with interrupts.

Can you give a link to the BasicStepper library that you use ?

The GY-521 module has no level shifters. That is a point of concern. I don’t care if it works, it is not 100% okay.

You can do more tests: with the Bluetooth module powered but not connected to pin 0 and 1; with the Bluetooth module connected but without reading the serial port. It could be a power request peak by the Bluetooth module. Can you buy an other Bluetooth module (not the same one) ? There must be a way to find what is weird about the Bluetooth module.

I'll come back to most of the things you said tomorrow. I did some more testing and found some things: - different apps and ways of sending the data from my phone causes the same issue (I am only sending data after receiving the Arduino data) - HOWEVER using my computer connected via Bluetooth works fine

I am sending the exact same string in bytes via Bluetooth from my phone as from my computer. I use the serial monitor via Bluetooth there. I know it's tough to tell, but any idea what might be causing it? Note that when sending from computer I don't wait for data from the Arduino but rather just spam the string into the serial monitor. Still over Bluetooth though.

Thanks for the answers, and thanks for the hint with the gy521. I'll change it. I didn't include level shifters til now, since the supplier who builds and sells these here didn't include any on the wiring schematic.. I don't use a stepper library, I'm sending step and dir Pin Signals myself via interrupts

  • different apps and ways of sending the data from my phone causes the same issue (I am only sending data after receiving the Arduino data)
  • HOWEVER using my computer connected via Bluetooth works fine

I am sending the exact same string in bytes via Bluetooth from my phone as from my computer. I use the serial monitor via Bluetooth there. I know it's tough to tell, but any idea what might be causing it? Note that when sending from computer I don't wait for data from the Arduino

What specific phone apps have you tried? What different ways? How is the phone app aware that it has received data from the Arduino and is good to go with a reply? Is there a way for you to work that handshake into the pc code?

This is starting to sound like the root cause is on the phone side.

I agree. No way to implement that to PC. The app notices when it’s received a full message via broadcast and then send one message back. I’ve written it myself. I’d like to know how the serial monitor sends data so I can apply that to my app…

Did further testing. Problem seems to become better if I set the sending rate on the phone on a fixed rate, and not make it depend on the arduino sending. Choosing 200ms as a sending interval worked for some time but in the end led to the arduino freezin, again. As if in some cases the arduino "isn't ready" to process even this. Maybe input buffer overflow? I can't really see how this is supposed to happen with a 200ms sending interval..

Choosing 200ms as a sending interval worked for some time but in the end led to the arduino freezin, again.

I want to adjust values via bluetooth. As soon as I start sending data via bluetooth (even if I send a zero, which doesnt influence the dynamics), the steppers act weirdly and the system becomes unstable (it’s a self balancing robot).

“Freezing” sounds like a different problem from “weird” stepper behaviour and unstable balance.

Please explain what you are seeing.

cattledog: "Freezing" sounds like a different problem from "weird" stepper behaviour and unstable balance.

Please explain what you are seeing.

It first behaved weridly (probably due to huge delays in the control loop as it can't be executed fast enough due to serial coms blocking it) and then "freezes", sometimes the motors stop moving, sometimes they move at a constant, low speed (so the timer interrupt still works, I think)