Skip to content
This repository has been archived by the owner on Mar 3, 2019. It is now read-only.

Commit

Permalink
bug fix: robot qui part en couille à fond la caisse
Browse files Browse the repository at this point in the history
sylvaing19 committed Apr 26, 2017
1 parent ec990f6 commit ee6da38
Showing 2 changed files with 24 additions and 14 deletions.
35 changes: 22 additions & 13 deletions teensy/loli_teensy/MotionControlSystem.cpp
Original file line number Diff line number Diff line change
@@ -413,6 +413,7 @@ void MotionControlSystem::updateTranslationSetpoint()

if (nextStopPoint == UINT16_MAX)
{
translationSetpoint = currentTranslation + UINT8_MAX * TRAJECTORY_STEP;
if (!undefinedStopPoint)
{
forwardTranslationPID.resetDerivativeError();
@@ -421,19 +422,9 @@ void MotionControlSystem::updateTranslationSetpoint()
backwardTranslationPID.resetIntegralError();
undefinedStopPoint = true;
}
translationSetpoint = currentTranslation + UINT8_MAX * TRAJECTORY_STEP;
}
else
{
if (undefinedStopPoint)
{
forwardTranslationPID.resetDerivativeError();
forwardTranslationPID.resetIntegralError();
backwardTranslationPID.resetDerivativeError();
backwardTranslationPID.resetIntegralError();
undefinedStopPoint = false;
}

uint8_t nbPointsToTravel = nextStopPoint - trajectoryIndex;
translationSetpoint = currentTranslation + nbPointsToTravel * TRAJECTORY_STEP;
if (nbPointsToTravel > 10)
@@ -443,13 +434,31 @@ void MotionControlSystem::updateTranslationSetpoint()
else
{
Position posConsigne = currentTrajectory[trajectoryIndex].getPosition();
translationSetpoint +=
float offset =
ABS(
(
(position.x - posConsigne.x) * cosf(posConsigne.orientation) +
(position.y - posConsigne.y) * sinf(posConsigne.orientation)
) / TICK_TO_MM
);
if (offset < TRAJECTORY_STEP)
{
translationSetpoint += (int32_t)offset;
}
else
{
translationSetpoint += TRAJECTORY_STEP / 2;
Log::warning("Position courante tres loin du point de trajectoire courant");
}
}

if (undefinedStopPoint)
{
forwardTranslationPID.resetDerivativeError();
forwardTranslationPID.resetIntegralError();
backwardTranslationPID.resetDerivativeError();
backwardTranslationPID.resetIntegralError();
undefinedStopPoint = false;
}
}
}
@@ -1059,8 +1068,8 @@ void MotionControlSystem::loadDefaultParameters()

maxAcceleration = 25;

forwardTranslationPID.setTunings(2.75, 0, 1);
backwardTranslationPID.setTunings(2.75, 0, 1);
forwardTranslationPID.setTunings(2.75, 0, 1.5);
backwardTranslationPID.setTunings(1.75, 0, 1);
leftSpeedPID.setTunings(0.6, 0.01, 20);
rightSpeedPID.setTunings(0.6, 0.01, 20);
curvatureCorrectorK1 = 0.1;
3 changes: 2 additions & 1 deletion teensy/loli_teensy/PID.h
Original file line number Diff line number Diff line change
@@ -69,7 +69,8 @@ class PID : public Printable
}

void resetDerivativeError() {
pre_error = 0;
// pre_error = error :
pre_error = (*setPoint) - (*input);
}
void resetIntegralError() {
integral = 0;

0 comments on commit ee6da38

Please sign in to comment.