Dear Forum:
I am having trouble interfacing a MPU6050 with the accelstepper library. I wish to use the MPU6050 to control the speed and acceleration of the stepper. This is moving back and forth a defined number of steps. Between each change of direction, the angle of the sensor (i.e. Y) needs to be measured and then used to change the speed. If the angle is too steep, the motor needs to stop and wait a bit.
I have checked the output angle of the MPU6050 and this is resonably close to the results of a digital protractor. The motor works fine but the speed does not change with a change of the angle. Is there some timing problem involved? I have had to deal with a few such problems on other projects.
This is my first attempt using a MPU6050 and also using accelstepper. I have done other projects with steppers and other motors with good success. (These have been accomplished without needing the help of the experts. ) Parts of the code have been copied from other sources. Some variables will be needed for other processes later when this problem has been solved. Proper callibration can come later as well. Sorry if the formatting is not correct. I am using 2.0.
Here is the code for this portion of the project:
#include <Arduino.h>
#include <AccelStepper.h>
AccelStepper stepper(1, 4, 5); // Stepper mode 1, Pins 4 and 5 see below:
#include <Wire.h>
const int dirPin = 4;
const int stepPin = 5;
bool toggle = false;
#define motor_steps 800
AccelStepper StepperMotor(AccelStepper::DRIVER, stepPin, dirPin);
// Define Conections
volatile unsigned int CumulativeRotationSteps, StepCounter, StatusCounter, UpperLimit, LowerLimit;
volatile int CheckPinsB, CheckPinsD, InitializeSteps;
volatile bool DirectionStatus, TurnMotorStatus, EnableStatus, Test1SwitchStatus, Test2SwitchStatus, FindBottom, Prepare_Status;
volatile bool InitializeStatus, Hall_MagasinUpFlag, Hall_SchopferDownFlag, Hall_MagasinUpInput, Hall_SchopferDownInput;
unsigned long PreviousStepperMicros = 0, NextStep, PullAcceleration, ReturnAcceleration, PullUpSpeed, ReturnDownSpeed, SpeedCorrection, MappedSpeed, ReturnDownSpeedFast, ReturnDownSpeedSlow;
volatile unsigned long CurrentMicros, NextSTepMicros, CycleDurationMicros, CalculatedMicros, TurnMotorStartMicros;
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ, GyroX, GyroY, GyroZ;
volatile float accAngleX, accAngleY, gyroAngleX, gyroAngleY, angleX, angleY;
volatile float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, Y_inverted, LastYPosition, CurrentYPosition, YPostionCorection ;
float elapsedTime, currentTime, previousTime;
int c = 0;
struct IMU {
float angleX, angleY;
unsigned long timeStamp;
};
IMU imu;
void setup() {
pinMode(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT);
//Serial.begin(57600);
initialize_MPU6050();
PORTD &= ~_BV(PD7); //digitalWrite(dirPin, LOW); //Set motor direction Cclockwise
StepperMotor.setCurrentPosition(0); //?????????????????????
PullAcceleration = 2000;
ReturnAcceleration = 1000;
PullUpSpeed = 2000;
ReturnDownSpeed = 1000;
SpeedCorrection = 50;
ReturnDownSpeedFast = 4000;
ReturnDownSpeedSlow = 1000;
UpperLimit = 2400;
LowerLimit = 100;
LastYPosition = 0;
CurrentYPosition = 0;
}
void loop() {
// Serial.print(Y_inverted); Serial.print(' '); Serial.print(CurrentYPosition); Serial.print(' '); Serial.println(MappedSpeed);;
read_IMU(); // from MPU6050
CalculateCurrentSpeedAcceleration();
PullUpRun();
read_IMU(); // from MPU6050
CalculateCurrentSpeedAcceleration();
if (Y_inverted < 45) {
MoveDown();
}
if (Y_inverted > 45) {
TopReached();
}
}
void initialize_MPU6050() {
Wire.begin(); // Initialize comunication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
// Configure Accelerometer
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Talk to the ACCEL_CONFIG register
Wire.write(0x00); //Set the register bits as 00010000 (+/- 2g full scale range)
Wire.endTransmission(true);
// Configure Gyro
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
Wire.write(0x00); // Set the register bits as 00010000 (1000dps full scale)//changed from 10!!!!!!!!!!!!!!!
Wire.endTransmission(true);
}
void calculate_IMU_error() { //This is probably not important at present but should be used later.
// We can call this funtion in the setup section to calculate the accelerometer and gury data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
// Read accelerometer values 1000times
while (c < 1000) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
//Divide the sum by 1000to get the error value
AccErrorX = AccErrorX / 1000;
AccErrorY = AccErrorY / 1000;
c = 0;
// Read gyro values 1000times
while (c < 1000) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 4, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 32.8);
GyroErrorY = GyroErrorY + (GyroY / 32.8);
c++;
}
//Divide the sum by 1000to get the error value
GyroErrorX = GyroErrorX / 1000;
GyroErrorY = GyroErrorY / 1000;
// Print the error values on the Serial Monitor
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
}
void read_IMU() {
// === Read acceleromter data === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
//For a range of +-8g, we need to divide the raw values by 4096, according to the datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 4096.0; // X-axis value
AccY = (Wire.read() << 8 | Wire.read()) / 4096.0; // Y-axis value
AccZ = (Wire.read() << 8 | Wire.read()) / 4096.0; // Z-axis value
// Calculating angle values using
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) + 1.15; // AccErrorX ~(-1.15) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - 0.52; // AccErrorX ~(0.5) Only Y axis used in this construction. X axis also possible
// === Read gyro data === //
previousTime = currentTime; // Previous time is stored before the actual time read
imu.timeStamp = millis(); // Current time actual time read
currentTime = float(imu.timeStamp);
elapsedTime = (currentTime - previousTime) / 200; // Divide by 1000 to get seconds //Changed from 1000!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!111111!!
Wire.beginTransmission(MPU);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 4, true); // Read 4 registers total, each axis value is stored in 2 registers
GyroX = (Wire.read() << 8 | Wire.read()) / 32.8; // For a 1000dps range we have to divide first the raw value by 32.8, according to the datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 32.8;
GyroX = GyroX + 1.85; //// GyroErrorX ~(-1.85)
GyroY = GyroY - 0.15; // GyroErrorY ~(0.15)
// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = GyroX * elapsedTime;
gyroAngleY = GyroY * elapsedTime;
// Complementary filter - combine acceleromter and gyro angle values
imu.angleX = 0.98 * (imu.angleX + gyroAngleX) + 0.02 * accAngleX; // X Value not needed depending on sensor direction
imu.angleY = 0.98 * (imu.angleY + gyroAngleY) + 0.02 * accAngleY;
Y_inverted = -imu.angleY; // resulting angle of sensor. Inversion (-) necessary depending on which part of sensor is up!
}
void CalculateCurrentSpeedAcceleration() {
MappedSpeed = map(Y_inverted, 0, 45, 15000, 1000);
PullUpSpeed = MappedSpeed;
if (Y_inverted > 25) {
ReturnDownSpeed = ReturnDownSpeedSlow;
}
else {
ReturnDownSpeed = ReturnDownSpeedFast;
}
PullAcceleration = MappedSpeed / 2;
ReturnAcceleration = ReturnDownSpeed / 2;
}
void PullUpRun() {
StepperMotor.setMaxSpeed(PullUpSpeed); // by setting this to the desired speed, the motor accelerates to this speed exactly without overshoot. StepperMotor.setMaxSpeed(PullUpSpeed);
StepperMotor.setAcceleration(PullAcceleration);
StepperMotor.moveTo(UpperLimit);
StepperMotor.runToPosition ();
}
void MoveDown() {
StepperMotor.setMaxSpeed(ReturnDownSpeed);
StepperMotor.setAcceleration(2400);
StepperMotor.moveTo(LowerLimit);
StepperMotor.runToPosition();
}
void TopReached() {
StepperMotor.setMaxSpeed(ReturnDownSpeedSlow);
StepperMotor.setAcceleration(2400);
StepperMotor.moveTo(LowerLimit);
StepperMotor.runToPosition();
do {
delay(700);
read_IMU();
} while (Y_inverted > 40);
}
