public virtual void OnMessageToDisplayPositionPidCorrection(PolarPidCorrectionArgs corr)
{
OnMessageToDisplayPositionPidCorrectionEvent?.Invoke(this, corr);
}
public void UpdateSpeedPolarPidCorrectionData(object sender, PolarPidCorrectionArgs e)
{
asservSpeedDisplay.UpdatePolarSpeedCorrectionValues(e.CorrPx, e.CorrPTheta,
e.CorrIx, e.CorrITheta,
e.CorrDx, e.CorrDTheta);
}
void PIDPosition(double elapsedTimeBetweenSamples)
{
if (ghostLocationRefTerrain != null)
{
double erreurXRefTerrain = ghostLocationRefTerrain.X - currentLocationRefTerrain.X;
double erreurYRefTerrain = ghostLocationRefTerrain.Y - currentLocationRefTerrain.Y;
currentLocationRefTerrain.Theta = Toolbox.ModuloByAngle(ghostLocationRefTerrain.Theta, currentLocationRefTerrain.Theta);
double erreurTheta = ghostLocationRefTerrain.Theta - currentLocationRefTerrain.Theta;
//Changement de repère car les asservissements se font dans le référentiel du robot
double erreurXRefRobot = erreurXRefTerrain * Math.Cos(currentLocationRefTerrain.Theta) + erreurYRefTerrain * Math.Sin(currentLocationRefTerrain.Theta);
double erreurYRefRobot = -erreurXRefTerrain *Math.Sin(currentLocationRefTerrain.Theta) + erreurYRefTerrain * Math.Cos(currentLocationRefTerrain.Theta);
////For testing purpose only
//double vxGhostRefRobot = ghostLocationRefTerrain.Vx * Math.Cos(currentLocationRefTerrain.Theta) + ghostLocationRefTerrain.Vy * Math.Sin(currentLocationRefTerrain.Theta);
//double vyGhostRefRobot = -ghostLocationRefTerrain.Vx * Math.Sin(currentLocationRefTerrain.Theta) + ghostLocationRefTerrain.Vy * Math.Cos(currentLocationRefTerrain.Theta);
//double vxRefRobot = vxGhostRefRobot;
//double vyRefRobot = vyGhostRefRobot;
//double vtheta = ghostLocationRefTerrain.Vtheta;
//Console.WriteLine("Temps entre sample Odometry : " + elapsedTimeBetweenSamples.ToString() + " ms");
double vxRefRobot = PID_X.CalculatePIDoutput(erreurXRefRobot, elapsedTimeBetweenSamples);
double vyRefRobot = PID_Y.CalculatePIDoutput(erreurYRefRobot, elapsedTimeBetweenSamples);
double vtheta = PID_Theta.CalculatePIDoutput(erreurTheta, elapsedTimeBetweenSamples);
//On regarde si la position du robot est proche de la position du ghost
double seuilToleranceEcartGhost = 1.0;
if (Math.Sqrt(Math.Pow(erreurXRefTerrain, 2) + Math.Pow(erreurYRefTerrain, 2) + Math.Pow(erreurTheta / 2, 2)) < seuilToleranceEcartGhost)
{
//Si c'est le cas, le robot n'a pas rencontré de problème, on envoie les vitesses consigne.
OnSpeedConsigneToRobot(robotId, (float)vxRefRobot, (float)vyRefRobot, (float)vtheta);
}
else
{
if (!isUsingXboxControler)
{
//Sinon, le robot a rencontré un obstacle ou eu un problème, on arrête le robot et on réinitialise les correcteurs et la ghostLocation
OnCollision(robotId, currentLocationRefTerrain);
OnSpeedConsigneToRobot(robotId, 0, 0, 0);
ghostLocationRefTerrain = currentLocationRefTerrain;
PIDPositionReset();
OnPidSpeedReset(robotId);
}
}
PolarPidCorrectionArgs correction = new PolarPidCorrectionArgs();
correction.CorrPx = PID_X.correctionP;
correction.CorrIx = PID_X.correctionI;
correction.CorrDx = PID_X.correctionD;
correction.CorrPy = PID_Y.correctionP;
correction.CorrIy = PID_Y.correctionI;
correction.CorrDy = PID_Y.correctionD;
correction.CorrPTheta = PID_Theta.correctionP;
correction.CorrITheta = PID_Theta.correctionI;
correction.CorrDTheta = PID_Theta.correctionD;
OnMessageToDisplayPositionPidCorrection(correction);
}
PolarPIDSetupArgs PositionPidSetup = new PolarPIDSetupArgs();
PositionPidSetup.P_x = PID_X.Kp;
PositionPidSetup.I_x = PID_X.Ki;
PositionPidSetup.D_x = PID_X.Kd;
PositionPidSetup.P_y = PID_Y.Kp;
PositionPidSetup.I_y = PID_Y.Ki;
PositionPidSetup.D_y = PID_Y.Kd;
PositionPidSetup.P_theta = PID_Theta.Kp;
PositionPidSetup.I_theta = PID_Theta.Ki;
PositionPidSetup.D_theta = PID_Theta.Kd;
PositionPidSetup.P_x_Limit = PID_X.ProportionalLimit;
PositionPidSetup.I_x_Limit = PID_X.IntegralLimit;
PositionPidSetup.D_x_Limit = PID_X.DerivationLimit;
PositionPidSetup.P_y_Limit = PID_Y.ProportionalLimit;
PositionPidSetup.I_y_Limit = PID_Y.IntegralLimit;
PositionPidSetup.D_y_Limit = PID_Y.DerivationLimit;
PositionPidSetup.P_theta_Limit = PID_Theta.ProportionalLimit;
PositionPidSetup.I_theta_Limit = PID_Theta.IntegralLimit;
PositionPidSetup.D_theta_Limit = PID_Theta.DerivationLimit;
OnMessageToDisplayPositionPidSetup(PositionPidSetup);
}
public void OnMessageToDisplayPositionPidCorrectionReceived(object sender, PolarPidCorrectionArgs e)
{
asservPositionDisplay.UpdatePolarSpeedCorrectionValues(e.CorrPx, e.CorrPTheta, e.CorrIx, e.CorrITheta, e.CorrDx, e.CorrDTheta);
}
