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 ;

}