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

    
  }


}

Wednesday, December 7, 2016

Robbie 1-4 six wheels 2WD



Robbie uses a atmega2560 with a RAMPS1.4 board to control 5 stepper motors.\

http://www.thingiverse.com/thing:1742052

3 variable regulators
4s 3300mA lipo battery
Ramps1.4 board
ATMega2560 board.
PCduino nano3b board running Armbian
32 Gb SDcard
USB 4 port hub
FS-GT2 wireless controller
GY21 MPU6050 gyroscope/accelerometer I2C interface
UC28 12 Volt Projector
5 x A4988 Stepper drivers



Why use a projector?

The main reason robbie has a projector is to help the robot,using a camera and opencv, determine the terrain in front of itself and hence be able to navigate.




The following code is for the ATMEGA2560.


You must use Fastwire for this program to work!! Open I2Cdev.h and uncomment the Fastwire declaration to enable it.

#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 accelgyro(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(800, 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);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

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




}
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;

bool tripped=false;

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

int mode=0;
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;
            }
            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 mode=0;
            
          //Serial.println("Changed Mode...");
            }


            
             }
      }
   
  if(mode==0){
    

      tripped=false;
      //MPU6050_RA_GYRO_ZOUT_H
      state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
        //Serial.print("FAIL1");
        Fastwire::reset();
       
        // Fastwire::setup(400, 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=(angle1+atan2(ay,az)*180.0/M_PI)/2.0;
      
       
      
      //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++;

  }

}