VectorFloat AttitudeLoop::ComputePP(Quaternion qM, VectorFloat omegaM) {
Quaternion qErr;
VectorFloat axisErr;
Serial.print("Printing qRef ");
fQRef.print();
Serial.print("Printing qM ");
qM.print();
qErr = fQRef.conjugate() * qM;
Serial.print("Printing qErr ");
qErr.print();
if(qErr.w < 0) axisErr = VectorFloat(qErr);
else axisErr = -VectorFloat(qErr);
Serial.print("Printing axisErr ");
axisErr.print();
Serial.print("Printing omegaM ");
omegaM.print();
Serial.print("Printing torque without I ");
(axisErr*fPQ - omegaM*fPOmega).print();
fTorque = fI * (axisErr*fPQ - omegaM*fPOmega);
Serial.print("Printing fTorque ");
fTorque.print();
return fTorque;
}
void Calibrator::newPoint(int motor, float p, VectorFloat omega, VectorFloat alpha, VectorFloat acceleration, Quaternion q) {
cout << p;
omega.print();
alpha.print();
acceleration.print();
q.print();
fP[motor].push_back(p);
fOmega[motor].push_back(omega);
fAlpha[motor].push_back(alpha);
fA[motor].push_back(acceleration);
fQ[motor].push_back(q);
}
