void computeCurrentPositionUsingCoders(PidMotion* pidMotion) {
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
// 2 dependant Wheels (direction + angle)
float value0 = (float)getCoderValue(CODER_LEFT);
float value1 = (float)getCoderValue(CODER_RIGHT);
// Compute real position of wheel
thetaCurrentValues->position = computeTheta(value0, value1);
alphaCurrentValues->position = computeAlpha(value0, value1);
}
int main(void) {
initializeI2C();
affTexte("Test LCD\n");
initCoders();
setupInterrupt();
unsigned int count = 0;
char s[20];
while (1) {
count++;
if ((count % 60000) == 0) {
clrScreen();
int i;
for (i=0; i<2; i++) {
long coder = getCoderValue(i);
sprintf(s, "%x_", (int)(coder >> 16));
affTexte(s);
sprintf(s, "%x", (int)(coder & 0xFFFF));
affTexte(s);
carRetCur();
}
count = 0;
}
}
void printPosition(OutputStream* outputStream) {
// clearScreen();
appendCRLF(outputStream);
Position* p = getPosition();
appendStringAndDec(outputStream, "left=", getCoderValue(CODER_LEFT));
appendStringAndDec(outputStream, " | right=", getCoderValue(CODER_RIGHT));
println(outputStream);
printPoint(outputStream, &(p->pos), " mm");
appendStringAndAngleInDeg(outputStream, "ang:", p->orientation);
appendStringAndAngleInDeg(outputStream, "\r\nang init:", p->initialOrientation);
appendStringAndDecf(outputStream, "\r\nlastLeft:", lastLeft);
appendString(outputStream, " pulse");
appendStringAndDecf(outputStream, "\r\nlastRight:", lastRight);
appendString(outputStream, " pulse");
appendStringAndAngleInDeg(outputStream, "\r\nlastAng:", lastAngle);
println(outputStream);
}
void updateTrajectoryWithNoThreshold() {
signed long left = getCoderValue(CODER_LEFT);
signed long right = getCoderValue(CODER_RIGHT);
// do not use threshold
absoluteUpdateFromCoders(left, right, false, false);
}
void updateTrajectory() {
signed long left = getCoderValue(CODER_LEFT);
signed long right = getCoderValue(CODER_RIGHT);
// curveUpdateFromCoders(left, right);
absoluteUpdateFromCoders(left, right, true, false);
}