0

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.

  • Only some will look through all that code. Suggest to start simple (rip out all the if() code) and keep adding back in code until it stops working. First prove to us it is or is not a communications problem. Then go from there. – st2000 May 23 at 15:00
  • Oh, should add. It is a common problem that high current items such motors will cause the processors to reset / act unexpectedly. Power your motors off a different power source WRT to the processor's power source. – st2000 May 23 at 15:03
  • @st2000 what should I add? Is there any way to solve this problem? Yes, tried with both way when we are connecting a different power source, common ground connected, also tried with the power source. – uvan May 23 at 15:07
  • I think the problem has something to do with poor implementation. Even though I didn't go through all the code I found one that will cause the described behaviour: if y==5 the program will be stuck in the while(true) loop which contains no break condition. There might be other mistakes but the symptoms you descibe are quite vague and the code is so long... you should try to provide us with a minimal example – Sim Son May 23 at 16:12
  • if y==5 the program will be stuck in the while(true) loop which contains no break condition. It is not even reaching up to this case. I have checked this case with a simple U-turn and stop, it is working fine multiple times. I am checking small case I will update and reduce code length – uvan May 24 at 5:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.