double BH280::PositionControlC(int m)
{
LowPass(m,0.3);
double out=0.0;
double dt=diffclock(&now,&lc);
double Kf=0.5/20.0;
//P
double qd=Cons.cValues[m];
double qf=Kf*Deadzone(m,0.30);
double q= Meas.Position[m];
double e= qd-q-qf;
//Meas.m280[2][m]=qd-q-qf;
//D
double pqd=pCons.cValues[m];
double pqf=Kf*Deadzone(m,0.30);
double pq= pMeas.Position[m];
double pe= pqd-pqf-pq;
double de=pe/dt;
out=reg.P[m]*e;// + reg.D[m]*de;
if (out>reg.maxV[m]) out=reg.maxV[m];
if (out<-reg.maxV[m]) out=-reg.maxV[m];
return out;
}
void ApplyDeadzone(f64 deadzone, bool linear)
{
if (linear)
{
f64 fX = Deadzone(X, deadzone, dzk);
f64 fY = Deadzone(Y, deadzone, dzk);
SetPoint(fX, fY);
}
else
{
f64 radius = Deadzone(r, deadzone, dzk);
SetRadius(radius);
}
}
