public void GenerateMessage2WheelsPolarSpeedPIDSetup(object sender, PolarPIDSetupArgs e)
{
byte[] payload = new byte[48];
payload.SetValueRange(((float)(e.P_x)).GetBytes(), 0);
payload.SetValueRange(((float)(e.I_x)).GetBytes(), 4);
payload.SetValueRange(((float)(e.D_x)).GetBytes(), 8);
payload.SetValueRange(((float)(e.P_theta)).GetBytes(), 12);
payload.SetValueRange(((float)(e.I_theta)).GetBytes(), 16);
payload.SetValueRange(((float)(e.D_theta)).GetBytes(), 20);
payload.SetValueRange(((float)(e.P_x_Limit)).GetBytes(), 24);
payload.SetValueRange(((float)(e.I_x_Limit)).GetBytes(), 28);
payload.SetValueRange(((float)(e.D_x_Limit)).GetBytes(), 32);
payload.SetValueRange(((float)(e.P_theta_Limit)).GetBytes(), 36);
payload.SetValueRange(((float)(e.I_theta_Limit)).GetBytes(), 40);
payload.SetValueRange(((float)(e.D_theta_Limit)).GetBytes(), 44);
OnMessageToRobot((Int16)Commands.PC2R_2WheelsPolarSpeedPIDSetGains, 48, payload);
OnMessageToDisplaySpeedPolarPidSetup(e);
}
public virtual void OnMessageToDisplaySpeedPolarPidSetup(PolarPIDSetupArgs setup)
{
OnMessageToDisplaySpeedPolarPidSetupEvent?.Invoke(this, setup);
}
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 OnMessageToDisplayPositionPidSetupReceived(object sender, PolarPIDSetupArgs e)
{
asservPositionDisplay.UpdatePolarSpeedCorrectionGains(e.P_x, e.P_theta, e.I_x, e.I_theta, e.D_x, e.D_theta);
asservPositionDisplay.UpdatePolarSpeedCorrectionLimits(e.P_x_Limit, e.P_theta_Limit, e.I_x_Limit, e.I_theta_Limit, e.D_x_Limit, e.D_theta_Limit);
}
