Monday, November 13, 2017

Adaptive Filter Algorithm for Gryoscope MPU6050 and Arduino

To use this code FASTWIRE needs to be enabled in I2Cdev.h in the ./Arduino/libraries/I2Cdev/ folder.


This code Assumes the IMUZero program has been used to calibrate the MPU6050 before use here.

Arduino Nano attached to a MPU 6050.


#include <I2Cdev.h>

#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include <Wire.h>
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro2(0x69); / <-- use for AD0 high


////////////////////////////////////////////////////////////////////////

void setup() {
     Serial.begin(115200);
  // put your setup code here, to run once:
while(!Serial.available());

Fastwire::setup(400, false);


   accelgyro.initialize();


    if(accelgyro.testConnection()){
     
    }
    else {
    
    }

      Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -1131
    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -5595
    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 3855
    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 30
    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 44
    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 27
    Serial.print("\n");
   //     accelgyro.setXAccelOffset(0);
  //  accelgyro.setYAccelOffset(0);
  //  accelgyro.setZAccelOffset(0);
        accelgyro.setXGyroOffset(27);
   accelgyro.setYGyroOffset(27);
   accelgyro.setZGyroOffset(78);
  delay(1000); 
}
///////////////////////////////////////////////////////////////////////
#define M 100
int a1x[M],a1y[M],a1z[M];
float A1x=0.0,A1y=0.0,A1z=0.0;
int counter_m=0;

#define O 100
int a2x[O],a2y[O],a2z[O];
float A2x=0.0,A2y=0.0,A2z=0.0;
int counter_o=0;

unsigned long dt=0;
double fdt=0.0;
int counter=0;
int N=1000;

int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1x=0.0;
float angle1y=0.0;
float angle1z=0.0;

float angle2x=0.0;
float angle2y=0.0;
float angle2z=0.0;

double fax=0.0,faz=0.0,fay=0.0,fd=0.0;

float theta;
#define DUTY_MAX 20
bool tripped=false;
bool flash=true;
unsigned int duty_counter=0,duty_cycleR=0,duty_cycleG=0,duty_cycleB=0;
///////////////////////////////////////////////////////////////////////////
void loop() {
 // Serial.print(duty_counter%DUTY_MAX);       Serial.print("\t");

 // duty_counter++;
 
for(int m=0;m<M;m++){a1x[m]=0;a1y[m]=0;a1z[m]=0;}
//////////////////////////////////////////////
fax=0.0;faz=0.0;fay=0.0;counter=0;
do{
tripped=false;
 
      state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
         
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }
     state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
     state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
         
     if(state>=0){
     ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }


  if(!tripped){
          counter++;
       fax+=ax;
       fay+=ay;
       faz+=az;
  }
 //   Serial.print(ax);       Serial.print("\t");
 //      Serial.print(ay);       Serial.print("\t");
   //     Serial.print(az);       Serial.print("\t");
     // Serial.print( sqrt(faz*faz+fax*fax+fay*fay));  Serial.print("\t\t");

 
       }while(counter<N);
       fax=fax/float(N);
       fay=fay/float(N);
       faz=faz/float(N);
       angle1y=atan(fay/faz);
       angle1x=atan(-fax/sqrt(fay*fay+faz*faz));
       ////////////////////////////
    Serial.print(fax);       Serial.print("\t");
      Serial.print(fay);       Serial.print("\t");
      Serial.print(faz);        Serial.print("\t");
      Serial.print(sqrt(fax*fax+fay*fay+faz*faz)-16356.0); Serial.println(""); 
////////////////////////////////////////////////////////////////////////////////////////////////


do{



state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
         
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }
     state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
     state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
         
     if(state>=0){
     ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }





      state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_YOUT_H,2, BUFFER);//
         
     if(state>=0){
     gy= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }

      state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);//
         
     if(state>=0){
     gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }

      state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);//
         
     if(state>=0){
     gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);  
     }



 if(!tripped){
          if(dt!=0)fdt=(micros()-dt)/1000000.0;
          else fdt=0.0;
          dt=micros();
         
          counter_m++;
          if(counter_m>=M){
            counter_m=0;
            counter_o++;
            if(counter_o>=O)counter_o=0;
            if( abs(A1x-A2x)>30 || abs(A1y-A2y)>30 || abs(A1z-A2z)>30 ){
              if(counter_o==0)counter_o=O-1;
              else counter_o--;
            }
            else{
            a2x[counter_o]=A1x;
            a2y[counter_o]=A1y;
            a2z[counter_o]=A1z;
            }
          for(int o=0;o<O;o++){A2x+=a2x[o];A2y+=a2y[o];A2z+=a2z[o];}
A2x=(A2x/O);A2z=(A2z/O);A2y=A2y/O;
           
          }
          a1x[counter_m]=gx;
          a1y[counter_m]=gy;
          a1z[counter_m]=gz;
         
         // if(abs(gx)>110)angle1x+=M_PI/180.0*gx/131.0*fdt;
         // if(abs(gy)>110)angle1y+=M_PI/180.0*gy/131.0*fdt;
        //  if(abs(gz)>110)angle1z+=M_PI/180.0*gz/131.0*fdt;


          for(int m=0;m<M;m++){A1x+=a1x[m];A1y+=a1y[m];A1z+=a1z[m];}
A1x=(A1x/M);A1z=(A1z/M);A1y=A1y/M;

          if(abs(A1x-A2x)>30)angle1x+=M_PI/180.0*(A1x-A2x)/131.0*fdt;
          if(abs(A1y-A2y)>30)angle1y+=M_PI/180.0*(A1y-A2y)/131.0*fdt;
          if(abs(A1z-A2z)>30)angle1z+=M_PI/180.0*(A1z-A2z)/131.0*fdt;



 Serial.print(A2x);       Serial.print("\t");
      Serial.print(A2y);       Serial.print("\t");
      Serial.print(A2z);Serial.print("\t"); 
    Serial.print(angle1x*180.0/M_PI);       Serial.print("\t");
      Serial.print(angle1y*180.0/M_PI);       Serial.print("\t");
      Serial.print(angle1z*180.0/M_PI);Serial.print("\t");       
    Serial.print(A1x);       Serial.print("\t");
      Serial.print(A1y);       Serial.print("\t");
      Serial.print(A1z);Serial.print("\t");
       Serial.print(fdt*1000);Serial.println(""); 

 
 }

}while(true);






}

Monday, December 19, 2016

robbie1-4 dual gyros










#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif




#define XSTEP 54
#define XDIR  55
#define XEN   38

#define YSTEP 60
#define YDIR  61
#define YEN   56

#define ZSTEP 46
#define ZDIR  48
#define ZEN   62

#define E0STEP 26
#define E0DIR  28
#define E0EN   24

#define E1STEP 36
#define E1DIR  34
#define E1EN   30


// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
MPU6050 accelgyro2(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;

int pushButton1 = 57;
int pushButton2 = 58;
unsigned int t00=0,t01=0;
unsigned int t10=0,t11=0;
int dt1=0,dt2=0;

int buttonState1=false;
int buttonState2=false;


int val1=0,val2=0;
int count1=0;int maxcount1=0;
int count2=0;int maxcount2=0;
int count3=0;int maxcount3=0;





void setup() {
   pinMode(pushButton1, INPUT);
  pinMode(pushButton2, INPUT);
  
  pinMode(YEN,OUTPUT);
  pinMode(YDIR,OUTPUT);
  pinMode(YSTEP,OUTPUT);
  pinMode(XEN,OUTPUT);
  pinMode(XDIR,OUTPUT);
  pinMode(XSTEP,OUTPUT);
  
  digitalWrite(XEN,LOW);
  digitalWrite(XDIR,LOW);
  digitalWrite(XSTEP,LOW);
  digitalWrite(YEN,LOW);
  digitalWrite(YDIR,LOW);
  digitalWrite(YSTEP,LOW);
  
  pinMode(ZEN,OUTPUT);
  pinMode(ZDIR,OUTPUT);
  pinMode(ZSTEP,OUTPUT);
  digitalWrite(ZEN,LOW);
  digitalWrite(ZDIR,LOW);
  digitalWrite(ZSTEP,LOW);
  
  pinMode(E0EN,OUTPUT);
  pinMode(E0DIR,OUTPUT);
  pinMode(E0STEP,OUTPUT);
  digitalWrite(E0EN,LOW);
  digitalWrite(E0DIR,HIGH);
  digitalWrite(E0STEP,LOW);
  
  pinMode(E1EN,OUTPUT);
  pinMode(E1DIR,OUTPUT);
  pinMode(E1STEP,OUTPUT);
  digitalWrite(E1EN,LOW);
  digitalWrite(E1DIR,LOW);
  digitalWrite(E1STEP,LOW);
  
     Fastwire::setup(400, false);
   

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(115200);
//while(!Serial.available());
    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    accelgyro2.initialize();
    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println(accelgyro2.testConnection() ? "MPU6050 connection  2 successful" : "MPU6050 connection 2 failed");

    // use the code below to change accel/gyro offset values
    accelgyro.setXGyroOffset(19);
    accelgyro.setYGyroOffset(68);
    accelgyro.setZGyroOffset(88);
    accelgyro.setXAccelOffset(-3914);
    accelgyro.setYAccelOffset(210);
    accelgyro.setZAccelOffset(2680);

    accelgyro2.setXGyroOffset(19);
    accelgyro2.setYGyroOffset(68);
    accelgyro2.setZGyroOffset(88);
    accelgyro2.setXAccelOffset(-3914);
    accelgyro2.setYAccelOffset(210);
    accelgyro2.setZAccelOffset(2680);
    




}
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;float offset1=0.0;
float angle2=0.0;float offset2=60.0;


double angleY=0.0;
bool tripped=false;

bool counter_switch1=false;
bool counter_switch2=false;
bool counter_switch3=false;

int mode=5;
String command;


void loop() {



   if(Serial){
             if(Serial.available()){
              command=Serial.readString();

            if(command[0]=='M'){
              String temp=command.substring(1,command.length());
              int firstspace=temp.indexOf(" ");
              String one=temp.substring(0,firstspace);
              temp=temp.substring(firstspace+1,temp.length());
             int secondspace=temp.indexOf(" ");
            String two=temp.substring(0,secondspace);
              temp=temp.substring(secondspace+1,temp.length());
              maxcount1=one.toInt();
              maxcount2=two.toInt();
              maxcount3=temp.toInt();
              if(maxcount1>0){
               digitalWrite(XDIR,HIGH);
              }
               else{
                digitalWrite(XDIR,LOW);
              }
              if(maxcount2>0){
               digitalWrite(YDIR,HIGH);
              }
               else{
                digitalWrite(YDIR,LOW);
              }
              if(maxcount3>0){
               digitalWrite(E1DIR,HIGH);
              }
               else{
                digitalWrite(E1DIR,LOW);
              }
              maxcount1=abs(maxcount1);
              maxcount2=abs(maxcount2);
              maxcount3=abs(maxcount3);
              
              Serial.print(maxcount1);Serial.print(":");
              Serial.print(maxcount2);Serial.print(":");
              Serial.println(maxcount3);
              
               counter_switch1=true;
            counter_switch2=true;
            counter_switch3=true;
            count1=count2=count3=0;
            }
            if(command[0]=='O'){
              String temp=command.substring(1,command.length());
              int firstspace=temp.indexOf(" ");
              String one=temp.substring(0,firstspace);
              temp=temp.substring(firstspace+1,temp.length());
             int secondspace=temp.indexOf(" ");
            String two=temp.substring(0,secondspace);
              
              offset1=(float)one.toInt();
              offset2=(float)two.toInt();
              Serial.println(offset1);
            Serial.println(offset2);
            }
            else if(command[0]=='m'){
              command=command.substring(1,command.length());
              mode=command.toInt();
            if(mode==0){
              
              
              digitalWrite(XEN,HIGH);
              digitalWrite(YEN,HIGH);
            }
        
            else if(mode==1){
             
             
              digitalWrite(XEN,LOW);
              digitalWrite(YEN,LOW);
            
            }
            else if(mode==2){
            counter_switch1=false;
            counter_switch2=false;
            counter_switch3=false;
            maxcount1=60000;
            maxcount2=60000;
            maxcount3=60000;
            count1=count2=count3=0;
            }
            else if(mode==3){

              
            }
            else if(mode==4){Serial.print(offset2);Serial.println("  Mode 4");}
            else if(mode==5){Serial.println("  Mode 5");}
            
            else mode=0;
            
          //Serial.println("Changed Mode...");
            }
            else if(command[0]=='a'){

              offset1+=1.0;
              //if(offset1>10.0)offset1=10.0;
              
            }
            else if(command[0]=='z'){

              offset1-=1.0;
              //if(offset1<-10.0)offset1=-10.0;
            }
             else if(command[0]=='s'){
                
              offset2+=1.0;
              //if(offset2>10.0)offset2=10.0;
              Serial.println(offset2);
            }
            else if(command[0]=='x'){

              offset2-=1.0;
              //if(offset2<-10.0)offset2=-10.0;
            Serial.println(offset2);
            }
            

            
             }
      }
   //////////////////////////////////////////////////////////////////////////////////////
  if(mode==0){
    

      tripped=false;
  
      state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
   


      //  Serial.print(gz);       Serial.print(":");
       // Serial.print(az);       Serial.print(":");
       if(!tripped){
        angle1=atan2(ay,az)*180.0/M_PI+offset1;
      
       
      
      //Serial.print(angle1);Serial.print("\t");
        if(angle1>0.0){
             digitalWrite(ZDIR,HIGH);
         digitalWrite(E0DIR,LOW);
        }
         else{
            digitalWrite(ZDIR,LOW);
         digitalWrite(E0DIR,HIGH);
        }
        
        if(abs(angle1)<0.75){maxcount3=0;counter_switch3=false;}
        else {
          counter_switch3=true;
          maxcount3=(105.0-10.0*abs(angle1));
        if(maxcount3<5)maxcount3=5;
        }
        

     

       }else {counter_switch3=false;maxcount3=0;}

      // Serial.print(maxcount3);Serial.println(":");

     // for(int i=0;i<10;i++){
      
    
        if(counter_switch3==true && count3>=maxcount3){
          digitalWrite(ZSTEP,HIGH);
          digitalWrite(E0STEP,HIGH);
        count3=0;
       }
   

       delayMicroseconds(20);
      
        digitalWrite(ZSTEP,LOW);
          digitalWrite(E0STEP,LOW);
       
     //if(counter_switch3==true)
     count3++;
     
     
      
  }///////////////////////////////////////////////////////////////////////
  else if(mode==1){


      val1=digitalRead(pushButton1);
      val2=digitalRead(pushButton2);
      
      if(val1==HIGH && buttonState1==false){
        buttonState1=true; 
        t00=micros();
      }
      else if(val1==LOW && buttonState1==true){
        t01=micros();
        if(t01>t00)dt1=t01-t00-1600;
        buttonState1=false;
        
        if(dt1>0){
           digitalWrite(XDIR,HIGH);
        }
         else{
            digitalWrite(XDIR,LOW);
        }
       
        if(abs(dt1)<200)maxcount1=1000;
        else maxcount1=(1680-3*abs(dt1))/50;
        if (maxcount1>=0 && maxcount1<5)maxcount1=5;
        if(maxcount1<=-1)maxcount1=1000;
        
      }
    if(val2==HIGH && buttonState2==false){
        buttonState2=true; 
        t10=micros();
      }
      else if(val2==LOW && buttonState2==true){
        t11=micros();
        if(t11>t10){dt2=t11-t10-1600;}//Serial.println(dt2);}
        buttonState2=false;
        
        if(dt2>0){
           digitalWrite(YDIR,HIGH);
        }
         else{
            digitalWrite(YDIR,LOW);
        }
   
        if(abs(dt2)<200)maxcount2=1000;
        else maxcount2=(1680-3*abs(dt2))/50;
        if (maxcount2>=0 && maxcount2<5)maxcount2=5;
        if(maxcount2<=-1)maxcount2=1000;
        
      }
    

      if(count1>=maxcount1){
          digitalWrite(XSTEP,HIGH);
         count1=0;
        }

      if(count2>=maxcount2){
          digitalWrite(YSTEP,HIGH);
         count2=0;
        }

   
   

       delayMicroseconds(50);
       digitalWrite(YSTEP,LOW);
        digitalWrite(XSTEP,LOW);
        digitalWrite(ZSTEP,LOW);
          digitalWrite(E0STEP,LOW);
       count1++;
       count2++;  




  
  }
  ///////////////////////////////////////////////////////////////////////
  else if(mode==2){


    
        
        
       

    

      if(counter_switch1==true && count1>=maxcount1){
          digitalWrite(XSTEP,HIGH);
         count1=0;
        }

      if(counter_switch2==true && count2>=maxcount2){
          digitalWrite(YSTEP,HIGH);
         count2=0;
        }

   if(counter_switch3==true && count3>=maxcount3){
          digitalWrite(E1STEP,HIGH);
         count3=0;
        }
   

       delayMicroseconds(50);
       digitalWrite(YSTEP,LOW);
        digitalWrite(XSTEP,LOW);
       
       digitalWrite(E1STEP,LOW);
       count1++;
       count2++;  
       count3++;

  }
  ///////////////////////////////////////////////////////////////////////
  else if(mode==3){
  
  
  tripped=false;
      //MPU6050_RA_GYRO_ZOUT_H
      state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
        //Serial.print("FAIL1");
        Fastwire::reset();
       tripped=true;
        // Fastwire::setup(400, false);   
     }
  
  if(!tripped && abs(gx)>200)angleY+=double(gx)/(4.0*16384.0);
   Serial.println(angleY);
  
  
  
  }////////////////////////////////////////////////////////////////////////////////////
  else if(mode==4){//////////////////////////////////////////////////////////////////////
    

      tripped=false;
  
      state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x69, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
   


      //  Serial.print(gz);       Serial.print(":");
       // Serial.print(az);       Serial.print(":");
       if(!tripped){
        angle2=atan2(ay,az)*180.0/M_PI+offset2;
      
       
      
   //  Serial.print(angle2);Serial.print("\t");
        if(angle2>0.0){
             
         digitalWrite(E1DIR,LOW);
        }
         else{
            
         digitalWrite(E1DIR,HIGH);
        }
        
        if(abs(angle2)<0.75){maxcount3=0;counter_switch3=false;}
        else {
          counter_switch3=true;
          maxcount3=(50.0-5.0*abs(angle2));
        if(maxcount3<5)maxcount3=5;
        }
        

     

       }else {counter_switch3=false;maxcount3=0;}

     // Serial.print(maxcount3);Serial.println(":");

     // for(int i=0;i<10;i++){
      
    
        if(counter_switch3==true && count3>=maxcount3){
        
          digitalWrite(E1STEP,HIGH);
        count3=0;
       }
   

       delayMicroseconds(50);
      
       
          digitalWrite(E1STEP,LOW);
       
     //if(counter_switch3==true)
     count3++;
     
     
      
  }///////////////////////////////////////////////////////////////////////
  else if(mode==5){

    tripped=false;
  
      state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
   


      //  Serial.print(gz);       Serial.print(":");
       // Serial.print(az);       Serial.print(":");
       if(!tripped){
        angle1=atan2(ay,az)*180.0/M_PI+offset1;
      
       
      
      //Serial.print(angle1);Serial.print("\t");
        if(angle1>0.0){
             digitalWrite(ZDIR,HIGH);
         digitalWrite(E0DIR,LOW);
        }
         else{
            digitalWrite(ZDIR,LOW);
         digitalWrite(E0DIR,HIGH);
        }
        
        if(abs(angle1)<0.75){maxcount3=0;counter_switch3=false;}
        else {
          counter_switch3=true;
          maxcount3=(105.0-10.0*abs(angle1));
        if(maxcount3<5)maxcount3=5;
        }
        
       }else {counter_switch3=false;maxcount3=0;}

       tripped=false;
  
      state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x69, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
   


      //  Serial.print(gz);       Serial.print(":");
       // Serial.print(az);       Serial.print(":");
       if(!tripped){
        angle2=atan2(ay,az)*180.0/M_PI+offset2;
      
       
      
   //  Serial.print(angle2);Serial.print("\t");
        if(angle2>0.0){
             
         digitalWrite(E1DIR,LOW);
        }
         else{
            
         digitalWrite(E1DIR,HIGH);
        }
        
        if(abs(angle2)<0.75){maxcount2=0;counter_switch2=false;}
        else {
          counter_switch2=true;
          maxcount2=(50.0-5.0*abs(angle2));
        if(maxcount2<5)maxcount2=5;
        }
        

     

       }else {counter_switch2=false;maxcount2=0;}

    
        if(counter_switch3==true && count3>=maxcount3){
          digitalWrite(ZSTEP,HIGH);
          digitalWrite(E0STEP,HIGH);
        count3=0;
       }
   
      if(counter_switch2==true && count2>=maxcount2){
        
          digitalWrite(E1STEP,HIGH);
        count2=0;
       }
   

       
      
       
          digitalWrite(E1STEP,LOW);
       
     
      
        digitalWrite(ZSTEP,LOW);
          digitalWrite(E0STEP,LOW);
       
     
     count3++;
     count2++;
     

    
  }


}