Changing speed of stepper at 100hz

[Saman]

Hi guys,

I am building a self-balancing robot using Arduino and stepper motors driven by A4988 drivers. I have the tilt angle fed to a PID loop which calculates a value to drive steppers to balance the robot.

I have set it it up so that tilt angle is calculated every 10ms and I was trying to change the speed of stepper motor also. The problem is that the stepper motor becomes jittery.

I think it is because I am trying to modify the speed too often.

I cannot understand this library and I need help.

void loop() {
  // Only run the controller once the time interval has passed
  if (millis() - timerValue > 10) {
    timerValue = millis();
    if (currentpos = prevpos + 1) {
      for (int i = 1; i < 100; i += 1) {
        myStepper.setSpeed(i * 10);
      }
      prevpos = currentpos;
    }
    Serial.print(currentpos);
    Serial.print('\n');
  }
  myStepper.run();
  currentpos = myStepper.currentPosition();
}

[Jim Larson]

The behavior of AccelStepper will depend on the settings of setMaxSpeed(), setAcceleration(), and moveTo(). Without knowing those (probably done in setup()),, it's impossible to say what's going on. I'm guessing that you don't really need the for loop with setSpeed() being called multiple times, and I don't see anywhere that you are changing your target position. But that's just a guess.

If you want to understand the AccelStepper library, have a look here:

https://hackaday.io/project/183279/details/

      -jim

[Saman]

I discovered your manual shortly after posting this and I definitely understand the library better.

Here is my complete code, excuse the mess.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <AccelStepper.h>
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
   ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
   ========================================================================= */

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

// Declare variables for PID
float Kp = 100;          // (P)roportional Tuning Parameter
float Ki = 0;          // (I)ntegral Tuning Parameter
float Kd = 0;          // (D)erivative Tuning Parameter
float iTerm = 0;       // Used to accumulate error (integral)
float lastTime = 0;    // Records the time the function was last called
float maxPID = 1000;    // The maximum value that can be output
float oldValue = 0;    // The last sensor value
float targetValue = 0;

// Variables for Time Keeper function:
//#define LOOP_TIME 50          // Time in ms (10ms = 100Hz)
unsigned long timerValue = 0;

//
//Stepper motors defined
const int dirPin = 4;
const int stepPin = 5;
//Stepper myStepper(800, 4, 5);
AccelStepper StepperR(1, 5, 4);
AccelStepper StepperL(1, 3, 6);
char inchar = 0;


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

/****** PID CONTROLLER *****/

float pid(float target, float current) {
  // Calculate the time since function was last called
  float thisTime = millis();
  float dT = thisTime - lastTime;
  lastTime = thisTime;

  // Calculate error between target and current values
  float error = target - current;

  // Calculate the integral term
  iTerm += error * dT;

  // Calculate the derivative term (using the simplification)
  float dTerm = (oldValue - current) / dT;

  // Set old variable to equal new ones
  oldValue = current;

  // Multiply each term by its constant, and add it all up
  float result = (error * Kp) + (iTerm * Ki) + (dTerm * Kd);

  // Limit PID value to maximum values
  if (result > maxPID) result = maxPID;
  else if (result < -maxPID) result = -maxPID;

  return result;
}

// ****************getAngle()*************
float getAngle() {
  if (!dmpReady) return 0;
  // read a packet from FIFO
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    return ypr[1] * 180 / M_PI;
#endif
  }
}

//**************
//----------speed control
//**************
float wait(float pid) {
  return (2000 / pid) + 200;
}

void movemotor(float waittime) {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(waittime);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(waittime);
}


////////////READ SERIAL///////////////////
//////////////////////////////////////////
void readSerial() {

    // Initiate all of the variables
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
  int changestate = 0;    // Which action needs to be taken?
  int no_before = 0;      // Numbers before decimal point
  int no_after = 0;     // Numbers after decimal point
  bool minus = false;     // See if number is negative
  inchar = Serial.read();   // Read incoming data

    if (inchar == 'P') changestate = 1;
    else if (inchar == 'I') changestate = 2;
    else if (inchar == 'D') changestate = 3;

    // Tell robot to calibrate the Centre of Gravity
    //else if (inchar == 'G') calibrateTargets();


    // Records all of the incoming data (format: 00.000)
    // And converts the chars into a float number
    if (changestate > 0){
        if (Serial.available() > 0){

            // Is the number negative?
            inchar = Serial.read();
            if(inchar == '-'){
                minus = true;
                inchar = Serial.read();
            }
            no_before = inchar - '0';

            if (Serial.available() > 0){
                inchar = Serial.read();

                if (inchar != '.'){
                    no_before = (no_before * 10) + (inchar - '0');

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                    }
                }

                if (inchar == '.'){
                    inchar = Serial.read();
                    if (inchar != '0'){
                        no_after = (inchar - '0') * 100;
                    }

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                        if (inchar != '0'){
                            no_after = no_after + ((inchar - '0') * 10);
                        }

                        if (Serial.available() > 0){
                            inchar = Serial.read();
                            if (inchar != '0'){
                                no_after = no_after + (inchar - '0');
                            }
                        }
                    }
                }
            }

            // Combine the chars into a single float
            float answer = float(no_after) / 1000;
            answer = answer + no_before;
            if (minus) answer = answer * -1;

            // Update the PID constants
            if (changestate == 1){
                Kp = answer;
                Serial.print("P - ");
            } else if (changestate == 2){
                Ki = answer;
                Serial.print("I - ");
            } else if (changestate == 3){
                Kd = answer;
                Serial.print("D - ");
            }
            Serial.print("Constant Set: ");
            Serial.println(answer, 3);

        } else {
            changestate = 0;
        }
    }
}
// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  Serial.begin(115200);
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(7);
  mpu.setYGyroOffset(15);
  mpu.setZGyroOffset(57);
  mpu.setXAccelOffset(943);
  mpu.setYAccelOffset(-629);
  mpu.setZAccelOffset(1578); // 1688 factory default for my test chip

  //Stepper acceleration
  // stepper1.setAcceleration(50);
  //stepper1.setMaxSpeed(1000);


  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    //Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
  //myStepper.setSpeed(4000);
  StepperL.setMaxSpeed(10000);
  StepperL.setAcceleration(10000);
  StepperR.setMaxSpeed(10000);
  StepperR.setAcceleration(10000);
  //myStepper.setAcceleration(5);
}

float currentValue = 0;
float currentpos = 0;
float prevpos = 0;


// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  // Only run the controller once the time interval has passed

  if (millis() - timerValue > 20) {
    timerValue = millis();
    currentValue = pid(0,getAngle());
    if(currentValue <20 && currentValue > -20)
      currentValue=0;
    StepperR.move(-currentValue);
    StepperL.moveTo(currentValue);
    Serial.print(currentValue);
    Serial.print('\n');
  }
 StepperR.run();
 StepperL.run();
 if (Serial.available() > 0){    // If new PID values are being sent by the interface
        readSerial();               // Run the read serial method
  }
}

[Jim Larson]

Your new code posting seems very different from your original posting How does it work now?

[Saman]

I tested with the 'UnoAccelStepperExper_1.ino' file from your manual and the steppers have no problem sustaining speeds as high as 1000 steps/sec.

But when I use the the program I posted earlier, the steppers won't reach a high enough speed and the robot ends up falling.

Any ideas why?

[Jim Larson]

First, since I am not familiar with the MPU6050, my suggestions may not be useful. But here goes.

The example program uses runSpeed(), not run(). That means you can achieve a higher speed - but 1000 steps/second should be possible using run(). 10000 is not possible.

I can't tell what you are using the readSerial() routine for, but it may be slowing your loop() and thus slowing the response.

Note that you are using stepperR.move() (a relative move) and stepperL.moveTo() (an absolute move). Not sure if this matters or not, but you may want them to be the same.

Once you are sure that the values being calculated are correct, you might want to remove the calls to Serial.print() in your update routine.

Remember that when you call move(), run() will attempt to move to that location until it is reached. If you call move() with a new value before the motor is stopped, movement will continue to the original target. Using high values for the speed and acceleration will get to the target faster, but run() will still have to slow the motors to a stop, then accelerate as the move to the new target happens.

If you have an oscilloscope, you may want to time various parts of your program. You can use digitalWrite to an unused pin to set it high when entering a section, then setting it back low. This can help you know where time is being spent in your program.

HTH

          -jim

[Ralph Hulslander]

So smnb..., when do we get to see the robot?

Ralph

[Saman]

Thanks Jim.

The idea is that I am reading the angle from MPU6050 every 20ms and putting it through a PID algorithm.

The number I get from PID is the speed of the motor which I am changing every 20ms also.

I also tried using PID output for move() and but as you said, I cannot keep updating it until the number of steps have been reached.

Both these methods don't seem to be working since the library does not like changing the target speed/steps too frequently.

If I increase the motor control delay to 100ms, it works but then it is too late for the motors to balance the robot.

Do you think I should go back to first principles and write a program to control the delay between pulses for the stepper?

On Sunday, April 10, 2022 at 7:18:35 PM UTC-4 jla...@pacifier.com wrote:

[Jim Larson]

Have you tried using setSpeed() to change speed and direction, then using runSpeed() to run the motors? I haven't tried this to see how quickly it can respond, but it might be worth a try.

   -jim

[Ralph Hulslander]

That  MPU6050 sure looks like fun.

Ralph

--
You received this message because you are subscribed to the Google Groups "accelstepper" group.
To unsubscribe from this group and stop receiving emails from it, send an email to accelstepper...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/accelstepper/31fcc330-1125-4a29-b199-c81a4ace9a18n%40googlegroups.com.