Friday, June 16, 2017

Video Arduino Nano prototype dual gyro handcontroller 2017

Arduino Nano prototype dual gyro handcontroller 2017

#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 LEDG 3
#define LEDB 5
#define LEDR 6

#define LEDR2 9
#define LEDG2 10
#define LEDB2 11


// 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() {
  // put your setup code here, to run once:
pinMode(LEDB,OUTPUT);
pinMode(LEDR,OUTPUT);
pinMode(LEDG,OUTPUT);

digitalWrite(LEDB,LOW);
digitalWrite(LEDG,HIGH);
digitalWrite(LEDR,HIGH);

pinMode(LEDB2,OUTPUT);
pinMode(LEDR2,OUTPUT);
pinMode(LEDG2,OUTPUT);
digitalWrite(LEDR2,HIGH);
digitalWrite(LEDG2,HIGH);
digitalWrite(LEDB2,LOW);


delay(1000);

Fastwire::setup(400, false);
    Serial.begin(115200);

   accelgyro.initialize();
    accelgyro2.initialize();

    if(accelgyro.testConnection()){
      digitalWrite(LEDR2,HIGH);
       digitalWrite(LEDG2,HIGH);
        digitalWrite(LEDB2,LOW);
    }
    else {
      digitalWrite(LEDR2,LOW);
      digitalWrite(LEDG2,HIGH);
      digitalWrite(LEDB2,HIGH);
    }
    if(accelgyro2.testConnection()){
      digitalWrite(LEDR,HIGH);
       digitalWrite(LEDG,HIGH);
        digitalWrite(LEDB,LOW);
    }
    else {
      digitalWrite(LEDR,LOW);
      digitalWrite(LEDG,HIGH);
      digitalWrite(LEDB,HIGH);
    }
}
///////////////////////////////////////////////////////////////////////


int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;
float angle2=0.0;
float angle3=0.0;
float angle4=0.0;



bool tripped=false;
bool flash=true;
unsigned int flash_counter=0,flash_count=0;
///////////////////////////////////////////////////////////////////////////
void loop() {


  flash_counter++;
  if(flash_counter>flash_count)flash_counter=0;
  if(flash_counter>flash_count/2)flash=true;
 else flash=false;


//////////////////////////////////////////////
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){
          angle1=atan2(ax,-az)*180/M_PI;
          angle2=atan2(ay,-az)*180/M_PI;
  
  //  Serial.print(ax);       Serial.print("\t");
    //    Serial.print(ay);       Serial.print("\t");
   //     Serial.print(az);       Serial.print("\t");
      
  }
       ////////////////////////////////////////////////////////////////////////////////////////////
    //  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;
     }
     state=I2Cdev::readBytes(0x69,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){
          angle3=atan2(ax,-az)*128/M_PI;
          angle4=atan2(ay,-az)*128/M_PI;
             Serial.print(angle1);       Serial.print("\t");
    Serial.print(angle2);       Serial.print("\t");
    Serial.print(angle3);       Serial.print("\t");
    Serial.print(angle4);       Serial.println("");
         
         flash_count=0.6*(abs(180-angle1)+abs(angle3));
          if(flash){
             digitalWrite(LEDB,LOW);
        
          digitalWrite(LEDG,HIGH);
            analogWrite(LEDR,(int)abs(128+angle2*128/180));
          }
          else {
            digitalWrite(LEDB,LOW);
            digitalWrite(LEDR,LOW);
          digitalWrite(LEDG,HIGH);
          }
          if(flash){
              digitalWrite(LEDB2,LOW);
        
          digitalWrite(LEDG2,HIGH);
            analogWrite(LEDR2,(int)abs(128+angle4*128/180));
          }
          else {
            
            digitalWrite(LEDB2,LOW);
            digitalWrite(LEDR2,LOW);
          digitalWrite(LEDG2,HIGH);
          }
          // Serial.println(angle2);
       }
}

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

    
  }


}