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); }