//Incoming Events
public void OnPositionDataProcessedEvent(object sender, PositionDataProcessedArgs e)
{
if (toggleFreeze == 0)
{
angularSpeedError = robot.vitesseAngulaireConsigne - e.VitesseAngulaireFromOdometry;
linearSpeedError = robot.vitesseLineaireConsigne - e.VitesseLineaireFromOdometry;
Osc_LinearSpeedOdometry.AddPointToLine((int)GraphLineID.LinearSpeed, e.Timestamp, e.VitesseLineaireFromOdometry);
Osc_AngularSpeedOdometry.AddPointToLine((int)GraphLineID.AngularSpeed, e.Timestamp, e.VitesseAngulaireFromOdometry);
Osc_AngularSpeedOdometry.AddPointToLine((int)GraphLineID.ErrorAngularSpeed, e.Timestamp, angularSpeedError);
Osc_LinearSpeedOdometry.AddPointToLine((int)GraphLineID.ErrorLinearSpeed, e.Timestamp, linearSpeedError);
}
}
private static void MsgProcessor_OnPositionDataProcessedEvent(object sender, PositionDataProcessedArgs e)
{
//ConsoleWriteColoredText("Angular Speed: " + e.VitesseAngulaireFromOdometry.ToString() + "\n" +
// "Linear Speed: " + e.VitesseLineaireFromOdometry + "\n" +
// "X position: " + e.XPositionFromOdometry + "\n" +
// "Y position: " + e.YPositionFromOdometry + "\n" +
// "angle[RAD]: " + e.AngleRadianFromOdometry + "\n" +
// "botTimestamp: " + e.Timestamp + "\n\n\n", ConsoleColor.Cyan);
robot.vitesseLineaireFromOdometry = e.VitesseLineaireFromOdometry;
robot.vitesseAngulaireFromOdometry = e.VitesseAngulaireFromOdometry;
robot.xPositionFromOdometry = e.XPositionFromOdometry;
robot.yPositionFromOdometry = e.YPositionFromOdometry;
}
