Hi all. I have made an equatorial star tracker which consists esp32, 28byj48, mpu6050 on main shaft (to measure angular velocity of shaft and maintains 0.0042 degrees/second), an oled, 2 push buttons to control motor speed. A strange problem i am facing from yesterday. Mpu angle decreases after 88.2 degree. It supposed to go upto 360. It affects the shaft speed as mpu is directly controls the steps of 28byj48. What could be the problem?
TIA
CODE:
#include <AccelStepper.h>
#include <mechButton.h>
#include <idlers.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <timeObj.h>
#include <MPU6050.h>
// Constants and Enums
constexpr int SCREEN_WIDTH = 128;
constexpr int SCREEN_HEIGHT = 64;
constexpr int OLED_RESET = -1;
constexpr int SCREEN_ADDRESS = 0x3C;
constexpr int MOTOR_PIN1 = 32;
constexpr int MOTOR_PIN2 = 33;
constexpr int MOTOR_PIN3 = 25;
constexpr int MOTOR_PIN4 = 26;
constexpr int motorInterfaceType = 8;
constexpr int BUTTON_PIN1 = 14;
constexpr int BUTTON_PIN2 = 12;
constexpr float INITIAL_MANUAL_SPEED = 760.34; // 0.00416667 degrees per second
constexpr float MPU_FACTOR = 700;
constexpr double DPS_FACTOR = 0.0042;
constexpr double SPEED_STEP = 1.0;
constexpr int MAX_SPEED = 950;
constexpr int MIN_SPEED = -100;
constexpr int MPU_READ_INTERVAL = 1000;
constexpr int DISPLAY_UPDATE_INTERVAL = 500;
constexpr int REGULATION_INTERVAL = 100;
constexpr int FILTER_SIZE = 10;
constexpr int SPEED_BUFFER_SIZE = 10;
constexpr float KP = 1.5;
constexpr float KI = 0.1;
constexpr float KD = 0.05;
enum MPUFullScaleGyroRange {
MPU_FS_250 = MPU6050_GYRO_FS_250,
MPU_FS_500 = MPU6050_GYRO_FS_500,
MPU_FS_1000 = MPU6050_GYRO_FS_1000,
MPU_FS_2000 = MPU6050_GYRO_FS_2000
};
enum MPUFullScaleAccelRange {
MPU_ACCEL_FS_2 = MPU6050_ACCEL_FS_2,
MPU_ACCEL_FS_4 = MPU6050_ACCEL_FS_4,
MPU_ACCEL_FS_8 = MPU6050_ACCEL_FS_8,
MPU_ACCEL_FS_16 = MPU6050_ACCEL_FS_16
};
enum MPUDLPFMode {
MPU_DLPF_BW_5 = MPU6050_DLPF_BW_5,
MPU_DLPF_BW_10 = MPU6050_DLPF_BW_10,
MPU_DLPF_BW_20 = MPU6050_DLPF_BW_20,
MPU_DLPF_BW_42 = MPU6050_DLPF_BW_42,
MPU_DLPF_BW_98 = MPU6050_DLPF_BW_98,
MPU_DLPF_BW_188 = MPU6050_DLPF_BW_188
};
// Global Variables
MPU6050 mpu;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
AccelStepper stepper(motorInterfaceType, MOTOR_PIN1, MOTOR_PIN3, MOTOR_PIN2, MOTOR_PIN4);
mechButton button1(BUTTON_PIN1);
mechButton button2(BUTTON_PIN2);
float manualSpeed = INITIAL_MANUAL_SPEED;
float mpuSpeed = INITIAL_MANUAL_SPEED;
float yaw, pitch, roll;
float prevPitch = 0;
float pitchRate = 0;
unsigned long startTime;
float targetDisplacement = 0;
float cumulativePitchChange = 0;
bool startCumulativeCalculation = false;
float pitchBuffer[FILTER_SIZE] = {0};
float pitchRateBuffer[FILTER_SIZE] = {0};
int bufferIndex = 0;
float pitchRateSum = 0;
int pitchRateCount = 0;
float averagePitchRate = 0;
float previousError = 0;
float integral = 0;
float averageMpuSpeed = 0;
float speedBuffer[SPEED_BUFFER_SIZE] = {0};
int speedBufferIndex = 0;
unsigned long lastMpuReadTime = 0;
unsigned long lastDisplayUpdateTime = 0;
unsigned long lastRegulationTime = 0;
timeObj longPressTimer1(1000);
timeObj longPressTimer2(1000);
// Function Prototypes
void setupMPU();
void setupDisplay();
void handleButton1();
void handleButton2();
void updateDisplay();
void updateSpeedFromMPU();
void calculateInclinationChangeRate();
void regulateManualSpeed();
void updateMPUData();
void setup() {
Serial.begin(115200);
setupDisplay();
setupMPU();
pinMode(BUTTON_PIN1, INPUT_PULLUP);
pinMode(BUTTON_PIN2, INPUT_PULLUP);
button1.setCallback(handleButton1);
button2.setCallback(handleButton2);
stepper.setMaxSpeed(MAX_SPEED);
stepper.setSpeed(manualSpeed);
startTime = millis();
}
void loop() {
idle();
stepper.runSpeed();
unsigned long currentMillis = millis();
if (currentMillis - lastMpuReadTime >= MPU_READ_INTERVAL) {
lastMpuReadTime = currentMillis;
updateSpeedFromMPU();
calculateInclinationChangeRate();
}
if (currentMillis - lastRegulationTime >= REGULATION_INTERVAL) {
lastRegulationTime = currentMillis;
regulateManualSpeed();
}
if (currentMillis - lastDisplayUpdateTime >= DISPLAY_UPDATE_INTERVAL) {
lastDisplayUpdateTime = currentMillis;
updateDisplay();
}
if ((millis() - startTime) >= 10000) {
startCumulativeCalculation = true;
}
}
void setupMPU() {
Wire.begin();
mpu.initialize();
if (!mpu.testConnection()) {
Serial.println("MPU6050 connection failed");
while (1);
}
mpu.setFullScaleGyroRange(MPU_FS_250);
mpu.setFullScaleAccelRange(MPU_ACCEL_FS_2);
mpu.setDLPFMode(MPU_DLPF_BW_5);
}
void setupDisplay() {
if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;);
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE);
display.setCursor(1, 1);
display.println("TrackUniv ESP WIFI");
display.display();
}
void handleButton1() {
if (button1.trueFalse()) {
longPressTimer1.start();
} else {
if (longPressTimer1.ding()) {
manualSpeed += SPEED_STEP * 4; // Increase speed by a larger step if long pressed
} else {
manualSpeed += SPEED_STEP;
}
manualSpeed = constrain(manualSpeed, MIN_SPEED, MAX_SPEED);
stepper.setSpeed(manualSpeed);
updateDisplay();
}
}
void handleButton2() {
if (button2.trueFalse()) {
longPressTimer2.start();
} else {
if (longPressTimer2.ding()) {
manualSpeed -= SPEED_STEP * 4; // Decrease speed by a larger step if long pressed
} else {
manualSpeed -= SPEED_STEP;
}
manualSpeed = constrain(manualSpeed, MIN_SPEED, MAX_SPEED);
stepper.setSpeed(manualSpeed);
updateDisplay();
}
}
void updateDisplay() {
unsigned long elapsedTime = (millis() - startTime) / 1000.0;
if (startCumulativeCalculation) {
targetDisplacement = DPS_FACTOR * (elapsedTime - 10); // Subtract 10 seconds from elapsedTime
} else {
targetDisplacement = 0; // Set targetDisplacement to 0 during the first 10 seconds
}
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0, 0);
display.print(abs(averagePitchRate), 6); // Display the average pitch rate
display.setTextSize(1);
display.setCursor(0, 20);
display.print("Step:"); display.print(manualSpeed, 1); display.print(" >> "); display.print(mpuSpeed, 1);
display.setCursor(0, 30);
display.print("Angle:"); display.print(pitch, 1); display.print(" Degree");
display.setCursor(0, 40);
display.print("Avg Speed:"); display.print(averageMpuSpeed, 2);
display.setCursor(0, 50);
display.print("Angle: "); display.print(abs(cumulativePitchChange), 2); display.print(" >> "); display.print(targetDisplacement, 2);
display.display();
}
void updateSpeedFromMPU() {
int16_t accelerationX, accelerationY, accelerationZ, gyroX, gyroY, gyroZ;
mpu.getMotion6(&accelerationX, &accelerationY, &accelerationZ, &gyroX, &gyroY, &gyroZ);
pitch = atan2(accelerationX, sqrt(accelerationY * accelerationY + accelerationZ * accelerationZ)) * 180 / PI;
roll = atan2(accelerationY, sqrt(accelerationX * accelerationX + accelerationZ * accelerationZ)) * 180 / PI;
yaw = atan2(accelerationZ, sqrt(accelerationX * accelerationX + accelerationY * accelerationY)) * 180 / PI;
pitchBuffer[bufferIndex] = pitch;
bufferIndex = (bufferIndex + 1) % FILTER_SIZE;
float pitchSum = 0;
for (int i = 0; i < FILTER_SIZE; i++) {
pitchSum += pitchBuffer[i];
}
pitch = pitchSum / FILTER_SIZE;
if (pitch < 0) {
pitch += 360;
}
Serial.print("Acceleration X: "); Serial.print(accelerationX);
Serial.print(" Y: "); Serial.print(accelerationY);
Serial.print(" Z: "); Serial.println(accelerationZ);
float error = DPS_FACTOR - pitchRate;
mpuSpeed = manualSpeed + error * MPU_FACTOR;
mpuSpeed = constrain(mpuSpeed, MIN_SPEED, MAX_SPEED);
stepper.setSpeed(mpuSpeed);
speedBuffer[speedBufferIndex] = mpuSpeed;
speedBufferIndex = (speedBufferIndex + 1) % SPEED_BUFFER_SIZE;
float speedSum = 0;
for (int i = 0; i < SPEED_BUFFER_SIZE; i++) {
speedSum += speedBuffer[i];
}
averageMpuSpeed = speedSum / SPEED_BUFFER_SIZE;
Serial.print("MPU Speed: "); Serial.println(mpuSpeed);
}
void calculateInclinationChangeRate() {
static unsigned long lastRateCalcTime = millis();
unsigned long currentMillis = millis();
float timeDiff = (currentMillis - lastRateCalcTime) / 1000.0;
// Check if 10 seconds have passed since the start
if ((currentMillis - startTime) >= 10000) {
startCumulativeCalculation = true;
}
if (timeDiff > 0) {
float currentPitchRate = (pitch - prevPitch) / timeDiff;
pitchRateBuffer[bufferIndex] = currentPitchRate;
float pitchRateSum = 0;
for (int i = 0; i < FILTER_SIZE; i++) {
pitchRateSum += pitchRateBuffer[i];
}
pitchRate = pitchRateSum / FILTER_SIZE;
// Update cumulative pitch change after 30 seconds
if (startCumulativeCalculation) {
cumulativePitchChange += pitch - prevPitch;
}
// Calculate average pitch rate as cumulative pitch change divided by elapsed time
averagePitchRate = cumulativePitchChange / ((millis() - startTime) / 1000.0);
}
// Reset the values if the pitch changes more than 12 degrees in 1 second
if (abs(pitch - prevPitch) > 12) {
cumulativePitchChange = 0;
targetDisplacement = 0;
startTime = millis(); // Reset start time to avoid continuous reset
}
prevPitch = pitch;
lastRateCalcTime = currentMillis;
}
void regulateManualSpeed() {
float error = targetDisplacement - abs(cumulativePitchChange); // Adjusted to cumulativePitchChange
float differencePercentage = (abs(error) / targetDisplacement) * 100;
integral += error;
float derivative = error - previousError;
float output = KP * error + KI * integral + KD * derivative;
if (differencePercentage > 5.0) { // Ensure difference does not exceed 5%
mpuSpeed = manualSpeed + output;
} else {
mpuSpeed = manualSpeed;
}
mpuSpeed = constrain(mpuSpeed, MIN_SPEED, MAX_SPEED);
stepper.setSpeed(mpuSpeed);
previousError = error;
Serial.print("Regulated MPU Speed: "); Serial.println(mpuSpeed);
}