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 }