For example... unsigned long long servo_timing[2]={0,0}; int A_data=0; void servo_ini() { Wire.beginTransmission(0x20); Wire.write(0x00); // address bank A Wire.write(0x00); Wire.endTransmission(); } void servo_set(unsigned int servo_tim,bool servo_mod){ if(servo_mod==1){ servo_tim=map(servo_tim,0,180,200,1900); } //if(servo_vector<=0)/lcd_tim=650;/servo_tim=220;/0 //if(servo_vector==1)/lcd_tim=1400;/servo_tim=1000;/90 //if(servo_vector>=2)/lcd_tim=2255;/servo_tim=1870;/180 if(micros()-servo_timing[0]>servo_tim) { A_data&=0b11111110; Wire.beginTransmission(0x20); Wire.write(0x12); // address bank A Wire.write(A_data); Wire.endTransmission(); if(micros()-servo_timing[1]>(20000-servo_tim)) { A_data|=0b00000001; Wire.beginTransmission(0x20); Wire.write(0x12); // address bank A Wire.write(A_data); Wire.endTransmission(); servo_timing[0]=micros(); servo_timing[1]=micros(); }

} }

For example...

unsigned long long servo_timing[2]={0,0};
int A_data=0;
void servo_ini()
{
  Wire.beginTransmission(0x20);
    Wire.write(0x00);      // address bank A
    Wire.write(0x00);
    Wire.endTransmission();
}
void servo_set(unsigned int servo_tim,bool servo_mod){
   if(servo_mod==1){
    servo_tim=map(servo_tim,0,180,200,1900);
   }
   //if(servo_vector<=0)/*lcd_tim=650;*/servo_tim=220;/0
   //if(servo_vector==1)/*lcd_tim=1400;*/servo_tim=1000;/90
   //if(servo_vector>=2)/*lcd_tim=2255;*/servo_tim=1870;/180
   if(micros()-servo_timing[0]>servo_tim)
   {
     A_data&=0b11111110;
     Wire.beginTransmission(0x20);
     Wire.write(0x12);      // address bank A
     Wire.write(A_data);
     Wire.endTransmission();
     if(micros()-servo_timing[1]>(20000-servo_tim))
     {
       A_data|=0b00000001;
       Wire.beginTransmission(0x20);
       Wire.write(0x12);      // address bank A
       Wire.write(A_data);
       Wire.endTransmission();
       servo_timing[0]=micros();
       servo_timing[1]=micros();
     }
   
  }
}