I have a stepper motor (kysan 1124090) turning CW and CCW based on the y direction of my accelerometer(MU6090). I am using an Arduino uno. It works great but after about 30 seconds there will be these pauses in the communication between the accelerometer and the stepper motor every 3-4 seconds. I can turn rotate the accelerometer and the stepper wont move for about half a second. I am wondering if its a storage issue, where after a while it has stock piled so much data that it needs that delay to keep it alive? When I verify the sketch it says I am only using 31% of the program storage space and 27% of dynamic memory. Maybe there is a way to get rid of all the data i dont need every time i run through my loop to get rid of this problem? I am attaching the code along with this. If anyone has any advice or things I could try, that would be awesome. Thanks.
#include <Wire.h>
#include "Kalman.h"
#define RESTRICT_PITCH
Kalman kalmanX;
Kalman kalmanY;
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle;
double compAngleX, compAngleY;
double kalAngleX, kalAngleY;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
Stepper stepper2(200, 8, 10, 9, 11);
int cantidad = 1;
int previous = 0;
void setup() {
stepper2.setSpeed(350);
Serial.begin(115200);
Wire.begin();
TWBR = ((F_CPU /400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while (i2cWrite(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100);
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000;
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
//Serial.print("kalAngle is:"); Serial.print(kalAngleX);
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt);
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
//Serial.print("pitch:"); Serial.println(pitch);
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt);
#endif
gyroXangle += gyroXrate * dt;
gyroYangle += gyroYrate * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll;
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
#if 0 // Set to 1 to activate
/*Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");*/
#endif
// Serial.print(kalAngleX); Serial.print("\t");
// Serial.print("\t");
// Serial.print(kalAngleY); Serial.print("\t");
#if 0
//Serial.print("\t");
//double temperature = (double)tempRaw / 340.0 + 36.53;
//Serial.print(temperature); Serial.print("\t");
#endif
//Serial.print("\r\n");
int valuex = kalAngleX;
//Serial.print("Valx:"); Serial.println(valuex);
stepper2.step(valuex - previous);
previous = valuex;
}