Hello Guys!
I'm having a problem combining my IMU with my stepper motor. I know it is a timing problem for sure. I have tried using milis() but didn't got it to work.. The speed of the stepper motor is very slow and is not increasing at all.
I would like to increase the speed of the stepper dependent of the value given by the IMU.
IMU Calculation alone -> Works perfect
Stepper alone -> Works perfect
Maybe someone could help me, it is driving me crazy.
I am using a CNC Shield (TMC2209 Driver) with Nema17 and a BMI160 IMU
I have attached my code:
#include <BMI160Gen.h>
#include <PID_v1.h>
#include <AccelStepper.h>
#define BAUDRATE 115200
// Sensor Data
#define SENSE_RATE 100
#define GYRO_RANGE 250
#define ACCL_RANGE 2
#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)
#define motorInterfaceType 1
double MotorspeedX = 0;
const int8_t i2c_addr = 0x69;
unsigned long startMillis; //some global variables available anywhere in the program
unsigned long currentMillis;
const unsigned long period = 0.0001;
//Stepper Motors
//X-Axis:
const int DirX1 = 6;
const int StepX1 = 3;
//const int StepX2 = 4;
//const int DirX2 = 7;
//Y-Axis:
//const int StepY1 = 2;
//const int DirY1 = 5;
//const int StepY2 = 12;
//const int DirY2 = 13;
AccelStepper myStepperX1(motorInterfaceType, StepX1, DirX1);
/*AccelStepper myStepperX2(motorInterfaceType, StepX2, DirX2);
AccelStepper myStepperY1(motorInterfaceType, StepY1, DirY1);
AccelStepper myStepperY2(motorInterfaceType, StepY2, DirY2);
*/
//Convert Raw Sensor Data
float convertRawGyro(int gRaw) {
// ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
float lsb_omega = float(0x7FFF) / GYRO_RANGE;
return gRaw / lsb_omega; // deg/sec
}
float convertRawAccel(int aRaw) {
// ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
float lsb_g = float(0x7FFF) / ACCL_RANGE;
return aRaw / lsb_g;
}
static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0;
// PID
double SetpointY, InputY, OutputY; // PID variables
double SetpointX, InputX, OutputX; // PID variables
double P = 600;
double I = 0;
double D = 0;
PID myPIDY(&InputY, &OutputY, &SetpointY, P, I, D, DIRECT); // Y axis PID
PID myPIDX(&InputX, &OutputX, &SetpointX, P, I, D, DIRECT); // X Axis PID
double deadSpot = 200;
void print_roll_pitch()
{
// read raw accl measurements from device
int rawXAcc, rawYAcc, rawZAcc; // x, y, z
BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
float accX = convertRawAccel(rawXAcc);
float accY = convertRawAccel(rawYAcc);
float accZ = convertRawAccel(rawZAcc);
float rad_a_roll = atan2(accY, accZ);
float rad_a_pitch = atan2(-accX, sqrt(accY * accY + accZ * accZ));
float accl_roll = rad_to_deg(rad_a_roll);
float accl_pitch = rad_to_deg(rad_a_pitch);
//Read raw gyro measurments:
int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
BMI160.readGyro(rawRoll, rawPitch, rawYaw);
float omega_roll = convertRawGyro(rawRoll);
float omega_pitch = convertRawGyro(rawPitch);
float omega_yaw = convertRawGyro(rawYaw);
unsigned long cur_mills = micros();
unsigned long duration = cur_mills - last_mills;
last_mills = cur_mills;
double dt = duration / 1000000.0; // us->s
if (dt > 0.1) return;
// Gyro data
gyro_roll += omega_roll * dt; // (ms->s) omega x time = degree
gyro_pitch += omega_pitch * dt; //Y
gyro_yaw += omega_yaw * dt; //Z
// Complementary filter data
comp_roll = 0.93 * (comp_roll + omega_roll * dt) + 0.07 * accl_roll; //X-Axis
comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; //Y-Axis
InputX = comp_roll;
myPIDX.Compute();
MotorspeedX = OutputX;
//Motorsteuerung X......
//...
//...
InputY = comp_pitch;
myPIDY.Compute();
//Motorsteuerung Y......
//...
//...
myStepperX1.setSpeed(OutputX);
myStepperX1.runSpeed();
/*
if (OutputY >= deadSpot) {
myStepperY1.setSpeed(OutputY);
myStepperY2.setSpeed(OutputY);
}
else if (OutputY <= deadSpot) {
myStepperY1.setSpeed(0);
myStepperY2.setSpeed(0);
}
*/
/* Serial.print(comp_roll);
Serial.print("/");
Serial.print(comp_pitch);
Serial.print("/");
Serial.print(OutputY);
Serial.print("/");
Serial.print(OutputX);
Serial.print("/");
Serial.print(SetpointY);
Serial.print("/");
*/
}
void setup() {
myStepperX1.setMaxSpeed(4000);
myStepperX1.setSpeed(0);
Serial.begin(115200);
BMI160.begin(BMI160GenClass::I2C_MODE, i2c_addr);
/* pinMode(StepX1,OUTPUT);
pinMode(DirX1,OUTPUT);
pinMode(StepX2,OUTPUT);
pinMode(DirX2,OUTPUT);
pinMode(StepY1,OUTPUT);
pinMode(DirY1,OUTPUT);
pinMode(StepY2,OUTPUT);
pinMode(DirY2,OUTPUT);
*/
// myStepperX2.setMaxSpeed(5000);
// myStepperX2.setSpeed(4000);
// myStepperY1.setMaxSpeed(5000);
// myStepperY2.setMaxSpeed(5000);
BMI160.setGyroRate(SENSE_RATE);
BMI160.setAccelerometerRate(SENSE_RATE);
BMI160.setGyroRange(GYRO_RANGE);
BMI160.setAccelerometerRange(ACCL_RANGE);
BMI160.autoCalibrateGyroOffset();
BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
SetpointY = 0; // Desired value for PID loops
SetpointX = 0;
myPIDY.SetMode(AUTOMATIC);
myPIDY.SetOutputLimits(-4000, 4000); // limits for PID loops
myPIDX.SetMode(AUTOMATIC);
myPIDX.SetOutputLimits(-4000, 4000);
}
void loop() {
print_roll_pitch();
}