The following is the code where I tried to implement:
The user types an angle to which the Step Motor would rotate and MPU6050 senses the angle.
Also, there is an offsetMode flag introduced by me to adjust the motor needle to zero degree reference.
The problem occurs when I give inputs, it works for sometime and after that, don't know when and why, but the input doesn't get accepted when i send it. Is it because of readString
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <LiquidCrystal.h>
// MPU6050 setup
MPU6050 mpu;
bool dmpReady = false;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[512];
Quaternion q;
VectorFloat gravity;
float ypr[3];
float yaw, pitch, roll;
float prevYaw = 0;
float currentYaw = 0;
float difference = 0;
float inputAngle;
bool offsetMode = false; // Flag to track offset adjustment mode
// Stepper motor setup
const int coilA1 = 8;
const int coilA2 = 9;
const int coilB1 = 10;
const int coilB2 = 11;
int currentStep = 0;
int targetStep = 0;
const int stepDelay = 25;
const int stepCount = 4;
const int steps[stepCount][4] = {
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
bool motorMoving = false;
// LCD setup
LiquidCrystal lcd(2, 3, 4, 5, 6, 7);
void setup() {
// Serial communication setup
Serial.begin(115200);
// LCD initialization
lcd.begin(16, 2);
lcd.print("Wait for 10 sec");
lcd.setCursor(0, 1);
lcd.print("until Calibration");
// MPU6050 initialization
Wire.begin();
Wire.setClock(400000);
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.CalibrateAccel();
mpu.CalibrateGyro();
delay(1000);
Serial.println("Calibration done");
// Clear LCD and show input prompt
lcd.clear();
lcd.print("Enter the Angle:");
// Stepper motor pin setup
pinMode(coilA1, OUTPUT);
pinMode(coilA2, OUTPUT);
pinMode(coilB1, OUTPUT);
pinMode(coilB2, OUTPUT);
// DMP initialization
if (devStatus == 0) {
mpu.setDMPEnabled(true);
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.println(F("DMP Initialization failed"));
lcd.clear();
lcd.print("DMP Init failed");
}
}
void loop() {
if (Serial.available() > 0) {
String input = Serial.readStringUntil('\n');
input.trim(); // Remove any leading/trailing whitespace
if (input == "0") {
inputAngle = 0;
targetStep = 0; // Return to initial position
} else if (input == "e") {
prevYaw = 0; // Set current position as zero reference
currentYaw = 0;
targetStep = 0;
currentStep = 0;
// mpu.CalibrateAccel();
// mpu.CalibrateGyro();
ypr[0] = 0;
ypr[1] = 0;
ypr[2] = 0;
Serial.println("Zero reference set.");
lcd.clear();
lcd.print("Zero ref set");
delay(200);
lcd.clear();
// lcd.print("Enter the Angle:");
} else if (input == "o") {
offsetMode = true; // Enter offset adjustment mode
Serial.println("Offset mode ON");
lcd.clear();
lcd.print("Offset Mode ON");
} else if (input == "x") {
offsetMode = false; // Exit offset adjustment mode
prevYaw = 0; // Reset prevYaw to zero
Serial.println("Offset mode OFF");
lcd.clear();
lcd.print("Offset Mode OFF");
delay(200);
lcd.clear();
lcd.print("Enter the Angle:");
} else {
inputAngle = input.toFloat();
inputAngle = constrain(inputAngle, -360, 360);
if (offsetMode) {
mpu.setDMPEnabled(false);
// In offset mode, rotate motor by the exact angle input
targetStep = currentStep + round((inputAngle / 1.8));
} else {
// In normal mode, rotate motor to the absolute input angle
targetStep = round((inputAngle / 1.8));
}
// Display the input angle on Serial Monitor and LCD
Serial.print(inputAngle);
Serial.print(",");
lcd.setCursor(0, 1);
lcd.print(inputAngle);
//lcd.print(" degrees "); // Clear any extra characters
delay(500);
}
// Start MPU6050 recording
// mpu.setDMPEnabled(true);
// Set motor moving flag
motorMoving = true;
}
// Move the motor to the target position
if (motorMoving) {
if (!offsetMode)
{
mpu.setDMPEnabled(true);
}
if (currentStep != targetStep) {
if (currentStep < targetStep) {
stepMotor(1);
currentStep++;
} else if (currentStep > targetStep) {
stepMotor(-1);
currentStep--;
}
delay(stepDelay);
// Record the MPU6050 data while the motor is moving
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
pitch = ypr[1] * 180 / M_PI;
roll = ypr[2] * 180 / M_PI;
}
} else {
// Stop MPU6050 recording
mpu.setDMPEnabled(false);
currentYaw = prevYaw + ypr[0] * 180 / M_PI;
// Display the sensor angles on Serial Monitor and LCD
Serial.print(currentYaw);
Serial.print(",");
// lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Yaw Measured = ");
lcd.setCursor(0,1);
lcd.print(currentYaw);
delay(500);
// lcd.setCursor(0, 1);
// lcd.print("Pitch: ");
// lcd.print(pitch);
// lcd.print(" Roll: ");
// lcd.print(roll);
// Calculate and display the difference
prevYaw = yaw;
difference = inputAngle - currentYaw;
Serial.println(difference);
if (offsetMode){
prevYaw = 0;
ypr[0] = 0;
currentYaw = 0;
}
delay(500); // Wait for 2 seconds to display the angles
// lcd.clear();
// lcd.print("The difference is");
// lcd.setCursor(0, 1);
// lcd.print(difference);
// Clear motor moving flag
motorMoving = false;
delay(200); // Wait for 2 seconds to display the difference
// Prompt for new angle input
lcd.clear();
// lcd.print("Enter the Angle:");
}
}
}
void stepMotor(int direction) {
static int stepIndex = 0;
stepIndex += direction;
if (stepIndex >= stepCount) {
stepIndex = 0;
} else if (stepIndex < 0) {
stepIndex = stepCount - 1;
}
setStep(steps[stepIndex][0], steps[stepIndex][1], steps[stepIndex][2], steps[stepIndex][3]);
}
void setStep(int a1, int a2, int b1, int b2) {
digitalWrite(coilA1, a1);
digitalWrite(coilA2, a2);
digitalWrite(coilB1, b1);
digitalWrite(coilB2, b2);
}
