public void UpdatePolarSpeedConsigneOnGraph(object sender, PolarSpeedArgs e)
{
oscilloX.AddPointToLine(0, currentTime, e.Vx);
oscilloTheta.AddPointToLine(0, currentTime, e.Vtheta);
//asservSpeedDisplay.UpdateConsigneValues(e.Vx, e.Vy, e.Vtheta);
}
public void OnOdometryRobotSpeedReceived(object sender, PolarSpeedArgs e)
{
locationRefTerrain.X = (double)(locationRefTerrain.X + (e.Vx / ConstVar.FREQ_IN_HZ) * Math.Cos(locationRefTerrain.Theta));
locationRefTerrain.Y = (double)(locationRefTerrain.Y + (e.Vx / ConstVar.FREQ_IN_HZ) * Math.Sin(locationRefTerrain.Theta));
locationRefTerrain.Theta = (double)(locationRefTerrain.Theta + e.Vtheta / ConstVar.FREQ_IN_HZ);
OnCalculatedLocation(robotId, locationRefTerrain);
}
//Input events
public void GenerateMessageSetSpeedConsigneToRobot(object sender, PolarSpeedArgs e)
{
byte[] payload = new byte[12];
payload.SetValueRange(((float)e.Vx).GetBytes(), 0);
payload.SetValueRange(((float)e.Vy).GetBytes(), 4);
payload.SetValueRange(((float)e.Vtheta).GetBytes(), 8);
OnMessageToRobot((Int16)Commands.PC2R_SpeedPolarSetConsigne, 12, payload);
OnSetSpeedConsigneToRobotReceived(e); //Pour affichage graphique supervision
}
public void SetRobotSpeed(object sender, PolarSpeedArgs e)
{
//Attention, les vitesses proviennent de l'odométrie et sont donc dans le référentiel robot
if (robotList.ContainsKey(e.RobotId))
{
robotList[e.RobotId].VxRefRobot = filterLowPassVxList[e.RobotId].Filter(e.Vx);
robotList[e.RobotId].VyRefRobot = filterLowPassVyList[e.RobotId].Filter(e.Vy);
robotList[e.RobotId].Vtheta = filterLowPassVThetaList[e.RobotId].Filter(e.Vtheta);
}
}
public void OnOdometryRobotSpeedReceived(object sender, PolarSpeedArgs e)
{
/// Ajoutez votre code de calcul de la nouvelle position ici
robotLocation.X = robotLocation.X + e.Vx * 1 / 50;
robotLocation.Y = robotLocation.Y + e.Vy * 1 / 50;
robotLocation.Theta = robotLocation.Theta + e.Vtheta * 1 / 50;
/// Ajoutez l'appel à l'event de transmission de la position calculée ici
OnCalculatedLocation(robotId, robotLocation);
}
public void OnOdometryRobotSpeedReceived(object sender, PolarSpeedArgs e)
{
if (robotId == e.RobotId)
{
currentOdoVxRefRobot = e.Vx;
currentOdoVyRefRobot = e.Vy;
currentOdoVxRefTerrain = currentOdoVxRefRobot * Math.Cos(currentGpsTheta) - currentOdoVyRefRobot * Math.Sin(currentGpsTheta);
currentOdoVyRefTerrain = currentOdoVxRefRobot * Math.Sin(currentGpsTheta) + currentOdoVyRefRobot * Math.Cos(currentGpsTheta);
currentOdoVtheta = e.Vtheta;
//double VxRefTerrain = currentOdoVx * Math.Cos(-kalmanLocationRefTerrain.Theta) - currentOdoVy * Math.Sin(-kalmanLocationRefTerrain.Theta);
//double VyRefRobot = currentOdoVx * Math.Sin(-kalmanLocationRefTerrain.Theta) + currentOdoVy * Math.Cos(-kalmanLocationRefTerrain.Theta);
//On extrapole les valeurs de position Gps en utilisant les vitesses mesurées
currentGpsXRefTerrain += currentOdoVxRefTerrain / fEch;
currentGpsYRefTerrain += currentOdoVyRefTerrain / fEch;
currentGpsTheta += currentOdoVtheta / fEch;
IterateFilter(currentGpsXRefTerrain, currentGpsYRefTerrain, currentGpsTheta, currentOdoVxRefTerrain, currentOdoVyRefTerrain, currentOdoVtheta, currentGyroVtheta);
//IterateFilter(1, 0, 0, 0, 0, 0, 0);
var output = GetEstimation();
kalmanLocationRefTerrain.X = output[0];
kalmanLocationRefTerrain.Vx = output[1];
double AxKalman = output[2];
kalmanLocationRefTerrain.Y = output[3];
kalmanLocationRefTerrain.Vy = output[4];
double AyKalman = output[5];
kalmanLocationRefTerrain.Theta = output[6];
kalmanLocationRefTerrain.Vtheta = output[7];
double AthetaKalman = output[8];
//Attention la location a renvoyer est dans le ref terrain pour les positions et dans le ref robot pour les vitesses
double kalmanLocationRefRobotVx = kalmanLocationRefTerrain.Vx * Math.Cos(-kalmanLocationRefTerrain.Theta) - kalmanLocationRefTerrain.Vy * Math.Sin(-kalmanLocationRefTerrain.Theta);
double kalmanLocationRefRobotVy = kalmanLocationRefTerrain.Vx * Math.Sin(-kalmanLocationRefTerrain.Theta) + kalmanLocationRefTerrain.Vy * Math.Cos(-kalmanLocationRefTerrain.Theta);
Location kalmanOutputLocation = new Location(kalmanLocationRefTerrain.X, kalmanLocationRefTerrain.Y, kalmanLocationRefTerrain.Theta,
kalmanLocationRefRobotVx, kalmanLocationRefRobotVy, kalmanLocationRefTerrain.Vtheta);
//Location kalmanOutputLocationRefTerrain = new Location(kalmanLocationRefTerrain.X, kalmanLocationRefTerrain.Y, kalmanLocationRefTerrain.Theta,
// kalmanLocationRefTerrain.Vx, kalmanLocationRefTerrain.Vy, kalmanLocationRefTerrain.Vtheta);
OnKalmanLocation(robotId, kalmanOutputLocation);
//KalmanMonitor.KalmanReceived();
}
}
public virtual void OnSetSpeedConsigneToRobotReceived(PolarSpeedArgs args)
{
OnSetSpeedConsigneToRobotReceivedEvent?.Invoke(this, args);
}
public virtual void OnPolarSpeedOrder(PolarSpeedArgs polarSpeed)
{
OnPolarSpeedOrderEvent?.Invoke(this, polarSpeed);
}
public void OnOdometryRobotSpeedReceived(object sender, PolarSpeedArgs e)
{
}
public void OnOdometryRobotSpeedReceived(object sender, PolarSpeedArgs e)
{
/// Ajoutez votre code de calcul de la nouvelle position ici
/// Ajoutez l'appel à l'event de transmission de la position calculée ici
}
