Speed up arduino

Using Arduino Programming Questions
14

here is my code. Thanks for the help =)

currently my code running at rate of 600Hz, but I need sampling rate up to 2kHz.
I check the sampling rate by setting high and low at one of the digital pin and monitor with scopemeter.

void Position_Count()
{
  A = digitalRead(22);
  B = digitalRead(24);
  double p=0;
  double a1 = 0;
  double b1 = 0; 
 
  if(A==1 && a1!=A)
  {
     if(B==1)
     {
       p=p-1;
       a1=1;
       b1=1;
     }
     count=p;
   }
    
   if(A==1 && a1!=A)
   {
      if(B==0)
      {
        p=p+1;
        a1=1;
        b1=0;
      }
      count=p;
   }

   if(A==0 && a1!=A)
   {
      if(B==1)
      {
        p=p+1;
        a1=0;
        b1=1;
      }
      count=p;
   }

   if(A==0 && a1!=A)
   {
      if(B==0)
      {
        p=p-1;
        a1=0;
        b1=0;
      }        
      count=p;
   }

   if(B==1 && b1!=B)
   {
      if(A==1)
      {
        p=p+1;
        b1=1;
        a1=1;
      }
      count=p;
   }
    
   if(B==1 && b1!=B)
   {
      if(A==0)
      {
        p=p-1;
        b1=1;
        a1=0;
      }
      count=p;
   }

   if(B==0 && b1!=B)
   {
      if(A==1)
      {
        p=p-1;
        b1=0;
        a1=1;
      }
      count=p;
   }
    
   if(B==0 && b1!=B)
   {
      if(A==0)
      {
        p=p+1;
        b1=0;
        a1=0;
      }
      count=p;
   }

}

void Force()
{
   fsrReading = analogRead(fsrPin);
 
  // analog voltage reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
   fsrVoltage = map(fsrReading, 0, 1023, 0, 5000);
  
    // The voltage = Vcc * R / (R + FSR) where R = 10K and Vcc = 5V
    // so FSR = ((Vcc - V) * R) / V        
    fsrResistance = 5000 - fsrVoltage;     // fsrVoltage is in millivolts so 5V = 5000mV
    fsrResistance *= 10000;                // 10K resistor
    fsrResistance = fsrVoltage;
 
    fsrConductance = 1000000;           // we measure in micromhos so 
    fsrConductance = fsrResistance;
 
    // Use the two FSR guide graphs to approximate the force
    if (fsrConductance <= 100)                  
    {
      fsrForce = fsrConductance + 20;
    } 
   
/    else
    {
      fsrForce = fsrConductance ;                
      fsrForce = 50;           
    }   
  
  fsrForce = fsrForce*3;
}

void Virtual_System(double maxampl, double SollAmpl, double Kp, double Damp, int Mass )
{
  double Amplitude, P_ampl;
  double Sum;
  double dampingValue, Damping;
  double Vset; 
  double acceleration, acceleration2;
  
//  Amplitude = sqrt(v*v + x*x);
  
  // C code for dampingValue, if...else 
  if(Amplitude >= maxampl && Amplitude <= maxampl+0.05)
  {
    dampingValue = 0.1*Vset;
  }
  else if(Amplitude > maxampl+0.05)
  {
    dampingValue = 0.3*Vset;
  }
  else if(Amplitude > maxampl+0.1)
  {
    dampingValue = 10*Vset;
  } 
  else
  {
    dampingValue = 0;
  }
  
  P_ampl = Amplitude - SollAmpl;
  
  Damping = P_ampl*Kp;  
  
  Sum = Vset*Damping;  
  
  acceleration = 0 - dampingValue - Sum - Vset*Damp - x*w ;//- ;//SatForce/Mass;
  
  double V_Conversion;
  
  if(v >= 0)
  {
   V_Conversion = 1 / ((1+V1)*(1+V1)) ;
  }
  else
  {
    V_Conversion = 1 / ((1-V1)*(1-V1)) ;
  }
  
  acceleration2 = acceleration*V_Conversion;
  
  // v = integrate of acceleration2
  v = v + acceleration2;  
  
  Potential();  
  w = Freq * 2*PI/60 ;

   Vset = v*w - PosGain;  
  
  //x = integrate of Vset    
  x = x + Vset*Sampletime ; 
  
}