I have a strange problem, I have Arduino UNO/MEGA and I need to get Gyro sensors's data and I want to see the data in the serial monitor. Seems like a simple task, I wrote a simple C program which collects data from Arduino through serial monitor I can see the data. Everything is working for few minutes and after that, it stops.
#include <Wire.h>
#include <MPU6050.h>
#include <Encoder.h>
MPU6050 mpu;
Encoder myEnc(11, 12);
long newP=0, newPosition=0, newPo=0, oldPo=0;//Encoder values
unsigned long currentTime, previousTime, elapsedTime;
double error, lastError, output, setPoint, cumError, rateError, kp=40, ki=0, kd=0;
// Timers
unsigned long timer = 0,timera=0;
float timeStep = 0.01;
//Yaw values
static double yaw = 0;
const int motor11=6;//LEFT MOTOR FW
const int motor12=7;//LEFT MOTOR BK
const int motor1pwm=3;
const int motor21=8;//RIGHT MOTOR FW
const int motor22=9;//RIGHT MOTOR BK
const int motor2pwm=5;
int thres1=120;//PWM Values
int thres2=120;
int flag1=0, flag2=0, flag3=0, flag4=1, flag5=0;
int x=0, y=0, save=0;
void setup()
{
Serial.begin(115200);
// Initialize MPU6050
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
mpu.calibrateGyro();
mpu.setThreshold(1);
pinMode(motor11, OUTPUT);
pinMode(motor12, OUTPUT);
pinMode(motor21, OUTPUT);
pinMode(motor22, OUTPUT);
pinMode(motor1pwm, OUTPUT);
pinMode(motor2pwm, OUTPUT);
}
void loop()
{
newP = myEnc.read()/4;
newPo=newP-newPosition;
Serial.print("Encoder ");
Serial.println(newPo);
Vector norm = mpu.readNormalizeGyro();
yaw = yaw + norm.ZAxis * timeStep;
if(abs(newPo)<=600) //Straight line motion
{
if(abs(newPo)%60==0)
{
if(flag3==0)//Increase x- coordinate
{
if(save<abs(newPo))
{
if(abs(newPo)>abs(oldPo))
{
x=x+1;
Serial.print("X= ");
Serial.println(x);
Serial.print("Y= ");
Serial.println(y);
if(x==10)
{
flag3=-1;
}
save=abs(newPo);
}
else if(abs(newPo)<=abs(oldPo))
{
save=abs(oldPo);
}
}
}
else if(flag4==0)//Decrease x-coordinate
{
if(save<abs(newPo))
{
if(abs(newPo)>abs(oldPo))
{
x=x-1;
Serial.print("X= ");
Serial.println(x);
Serial.print("Y= ");
Serial.println(y);
if(x==0)
{
flag4=1;
}
save=abs(newPo);
}
else if(abs(newPo)<=abs(oldPo))
{
save=abs(oldPo);
}
}
}
}
if(flag1==0)//Set Point
{
setPoint=yaw;
Serial.print("Set Point= ");
Serial.println(setPoint);
flag1=-1;
}
if(flag2==0)//slow start
{
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, 75);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, 75);
delay(500);
flag2=-1;
}
timer = millis();
currentTime = millis();
elapsedTime = currentTime - previousTime;
error = setPoint - yaw;
cumError = cumError+(error * elapsedTime);
rateError = (error - lastError)/elapsedTime;
output = kp*error+ki*cumError+kd*rateError;
lastError = error;
previousTime = currentTime;
if(error>0.001)
{
Serial.print("1. YAW= ");
Serial.println(yaw);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, thres2);
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
if(thres1+output<255)
{
analogWrite(motor2pwm, thres1+output);
}
else
{
analogWrite(motor2pwm, 255);
}
}
else if(error<-0.001)
{
Serial.print("2. YAW= ");
Serial.println(yaw);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
if(thres2-output<255)
{
analogWrite(motor1pwm, thres2-output);
}
else
{
analogWrite(motor1pwm, 255);
}
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, thres1);
}
else if((error>-0.001)&&(error<0.001))
{
Serial.print("3. YAW= ");
Serial.println(yaw);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, thres2);
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, thres1);
}
oldPo=newPo;
flag5=0;
delay((timeStep*200) - (millis() - timer));
}
else if(abs(newPo)>600)
{
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, 90);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, 90);
delay(300);
if(flag5==0)
{
y=y+1;
flag5=-1;
}
if(y%2==0)
{
while((abs(setPoint)-abs(yaw)<48)&&(yaw<0.2))
{
timera=millis();
Vector norm = mpu.readNormalizeGyro();
yaw = yaw + norm.ZAxis * timeStep;
Serial.print("Turning Yaw= ");
Serial.println(yaw);
digitalWrite(motor11, LOW);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, 120);
digitalWrite(motor21, HIGH);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, 120);
delay((timeStep*2000) - (millis() - timera));
}
flag3=0;
}
else if((y+1)%2==0)
{
while(abs(yaw)-abs(setPoint)<50)
{
timera=millis();
Vector norm = mpu.readNormalizeGyro();
yaw = yaw + norm.ZAxis * timeStep;
Serial.print("Turning Yaw= ");
Serial.println(yaw);
digitalWrite(motor11, HIGH);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, 120);
digitalWrite(motor21, LOW);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, 120);
delay((timeStep*2000) - (millis() - timera));
}
flag4=0;
if(y==5)
{
while(true)
{
digitalWrite(motor11, LOW);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, 70);
digitalWrite(motor21, LOW);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, 30);
}
}
}
digitalWrite(motor11, LOW);
digitalWrite(motor12, LOW);
analogWrite(motor1pwm, thres2);
digitalWrite(motor21, LOW);
digitalWrite(motor22, LOW);
analogWrite(motor2pwm, thres1);
flag1=0;
flag2=0;
newPosition = myEnc.read()/4;
flag5=0;
save=0;
oldPo=0;
delay(2000);
}
}
This code is supposed to calculate a distance travelled in a line using encoder (PID algorithm implemented using Gyroscope data for straight motion) after reaching desired position, machine takes a U-Turn.
It stop for a second after taking the U-Turn and then starts straight motion again. The problem I'm facing is that the gyroscope readings stop randomly while machine taking the turn. Due to this, the machine keeps rotating about that point without stopping.
The serial monitor just stops displaying the reading. This does not happen every time, and it is very random.
