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);
    accelgyro.setFullScaleGyroRange(0); //0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 =  +/- 2000 degrees/sec
  accelgyro.setFullScaleAccelRange(0);  //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 =  +/- 16g
  delay(1000); 
}
///////////////////////////////////////////////////////////////////////
#define M 20
#define O 20
float A2x=0.0,A2y=0.0,A2z=0.0;
float A3x=0.0,A3y=0.0,A3z=0.0;
float A1x=0.0,A1y=0.0,A1z=0.0;
int counter_m=0,counter_o=0;
bool calc_A3=false;


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++;

//////////////////////////////////////////////
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=180.0/M_PI*atan(fay/faz);
       angle1x=180.0/M_PI*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{

A2x=0.0;A2y=0.0;A2z=0.0;

counter_o=0.0;
do{

A1x=0.0;A1y=0.0;A1z=0.0;
counter_m=0;fdt=0.0;
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++;
    
          A1x+=gx;
          A1y+=gy;
          A1z+=gz;






 
 }

}while(counter_m<M);

A1x/=1.0*M;
A1y/=1.0*M;
A1z/=1.0*M;
if(calc_A3 && (abs(A1x-A3x)>15.0 || abs(A1y-A3y)>15.0 || abs(A1z-A3z)>50.0 )){
  angle1x+=fdt*A1x/131.0;
  angle1y+=fdt*A1y/131.0;
  angle1z+=fdt*A1z/131.0;
 // Serial.print("*");
  //counter_o=0;A2x=0.0;A2y=0.0;A2z=0.0;
}
else{
A2x+=A1x;
A2y+=A1y;
A2z+=A1z;
counter_o++;
}



/// Serial.print(abs(A1x-A3x));       Serial.print("\t");
 //     Serial.print(abs(A1y-A3y));       Serial.print("\t");
  //    Serial.print(abs(A1z-A3z));Serial.print("\t");
    //  Serial.print(A3x);       Serial.print("\t");
   //   Serial.print(A3y);       Serial.print("\t");
     // Serial.print(A3z);Serial.print("\t");
    Serial.print(angle1x);       Serial.print("\t");
     Serial.print(angle1y);       Serial.print("\t");
      Serial.print(angle1z);Serial.print("\n");       
  
     //  Serial.print(fdt*1000);Serial.println("");

}while(counter_o<O);
A3x=A2x/(1.0*O);
A3y=A2y/(1.0*O);
A3z=A2z/(1.0*O);
  
calc_A3=true;
}while(true);

}







Opengl gyro_cube.c

 
/* Copyright (c) Mark J. Kilgard, 1997. */

/* This program is freely distributable without licensing fees
   and is provided without guarantee or warrantee expressed or
   implied. This program is -not- in the public domain. */

/* This program was requested by Patrick Earl; hopefully someone else
   will write the equivalent Direct3D immediate mode program. */

#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <GL/glut.h>


GLfloat light_diffuse[] = {1.0, 0.0, 0.0, 1.0};  /* Red diffuse light. */
GLfloat light_position[] = {1.0, 5.0, -1.0, 0.0};  /* Infinite light location. */
GLfloat n[6][3] = {  /* Normals for the 6 faces of a cube. */
  {-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0},
  {0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0} };
GLint faces[6][4] = {  /* Vertex indices for the 6 faces of a cube. */
  {0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4},
  {4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} };
GLfloat v[8][3];  /* Will be filled in with X,Y,Z vertexes. */

//serial descriptor
int fd;
char *portname = "/dev/ttyUSB0";

int
set_interface_attribs (int fd, int speed, int parity)
{
        struct termios tty;
        memset (&tty, 0, sizeof tty);
        if (tcgetattr (fd, &tty) != 0)
        {
               // error_message ("error %d from tcgetattr", errno);
                return -1;
        }

        cfsetospeed (&tty, speed);
        cfsetispeed (&tty, speed);

        tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;     // 8-bit chars
        // disable IGNBRK for mismatched speed tests; otherwise receive break
        // as \000 chars
        tty.c_iflag &= ~IGNBRK;         // disable break processing
        tty.c_lflag = 0;                // no signaling chars, no echo,
                                        // no canonical processing
        tty.c_oflag = 0;                // no remapping, no delays
        tty.c_cc[VMIN]  = 0;            // read doesn't block
        tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout

        tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl

        tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
                                        // enable reading
        tty.c_cflag &= ~(PARENB | PARODD);      // shut off parity
        tty.c_cflag |= parity;
        tty.c_cflag &= ~CSTOPB;
        tty.c_cflag &= ~CRTSCTS;

        if (tcsetattr (fd, TCSANOW, &tty) != 0)
        {
                //error_message ("error %d from tcsetattr", errno);
                return -1;
        }
        return 0;
}

void
set_blocking (int fd, int should_block)
{
        struct termios tty;
        memset (&tty, 0, sizeof tty);
        if (tcgetattr (fd, &tty) != 0)
        {
                //error_message ("error %d from tggetattr", errno);
                return;
        }

        tty.c_cc[VMIN]  = should_block ? 1 : 0;
        tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout
          tcsetattr (fd, TCSANOW, &tty);
}


void
drawBox(void)
{
  int i;

  for (i = 0; i < 6; i++) {
    glBegin(GL_QUADS);
    glNormal3fv(&n[i][0]);
    glVertex3fv(&v[faces[i][0]][0]);
    glVertex3fv(&v[faces[i][1]][0]);
    glVertex3fv(&v[faces[i][2]][0]);
    glVertex3fv(&v[faces[i][3]][0]);
    glEnd();
  }
}




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

void
display(void)
{
 char buffer1[100],buffer2[100],buffer3[100];
  float A1x=0.0,A1y=0.0,A1z=0.0;
  char line[200];
   
char *pch;
char *pch1;
char *pch2;
char in;
ssize_t length;


int i=0;

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
printf("START\n");

 do{
length = read(fd, &in, 1);
//printf("%d",length);

if(length==1){
line[i]=in;
i++;
}
//printf("%d",i);

}while( in!='\n');

line[i]='\0';
//printf("%s\n",line);


pch=strchr(line,'\t');
      strncpy(buffer1,line,pch-line+1);
A1x=atof(buffer1);

   pch1=strchr(pch+1,'\t');
strncpy(buffer2,pch,pch1-pch+1);
A1y=atof(buffer2);

   pch2=strchr(pch1+1,'\n');
strncpy(buffer3,pch1,pch2-pch1+1);
A1z=atof(buffer3);

 printf("%g\t%g\t%g\n",A1x,A1y,A1z);
 glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
 gluLookAt(0.0, 0.0, 5.0,  /* eye is at (0,0,5) */
    0.0, 0.0, 0.0,      /* center is at (0,0,0) */
    0.0, 1.0, 0.);      /* up is in positive Y direction */
//glTranslatef(0.0, 0.0, 1.0);
 glRotatef(A1z, 0.0, 1.0, 0.0);
 
 glRotatef(-A1x+90, 1.0, 0.0, 0.0);
  drawBox();
  glutSwapBuffers();
glutPostRedisplay();
}



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


void
init(void)
{
  /* Setup cube vertex data. */
  v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1;
  v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1;
  v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1;
  v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1;
  v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1;
  v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1;

  /* Enable a single OpenGL light. */
  glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHTING);

  /* Use depth buffering for hidden surface elimination. */
  glEnable(GL_DEPTH_TEST);

  /* Setup the view of the cube. */
  glMatrixMode(GL_PROJECTION);
  gluPerspective( /* field of view in degree */ 40.0,
    /* aspect ratio */ 1.0,
    /* Z near */ 1.0, /* Z far */ 10.0);
 glMatrixMode(GL_MODELVIEW);
  gluLookAt(0.0, 0.0, 5.0,  /* eye is at (0,0,5) */
    0.0, 0.0, 0.0,      /* center is at (0,0,0) */
    0.0, 1.0, 0.);      /* up is in positive Y direction */

  /* Adjust cube position to be asthetic angle. */
  //glTranslatef(0.0, 0.0, -1.0);
  //glRotatef(60, 1.0, 0.0, 0.0);
  //glRotatef(-20, 0.0, 0.0, 1.0);
}

int
main(int argc, char **argv)
{

fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
        printf ("error opening port\n");
        return 1;
}
printf("port opened...");
set_interface_attribs (fd, B115200, 0);  // set speed to 115,200 bps, 8n1 (no parity)
set_blocking (fd, 0); 

//printf("Writing port...\n");   
//write (fd, "\n", 2);           // send 7 character greeting

//usleep ((2) * 100); 
printf("init glut...");
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutCreateWindow("red 3D lighted cube");
  glutDisplayFunc(display);
  init();
  glutMainLoop();
  return 0;             /* ANSI C requires main to return int. */
}

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++;
     

    
  }


}