protected virtual void OnOdometryUpdate(OdometryUpdateEventArgs e) { if (OdometryUpdated != null) { OdometryUpdated(this, e); } }
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e) { Gdk.Threads.Enter(); try { robot.UpdateOdometry(e); } finally { Gdk.Threads.Leave(); } }
private void Read() { while (serialPort.IsOpen) { try { string message = serialPort.ReadLine(); switch (message [0]) { case 'o': string[] parameters = message.Split(','); OdometryUpdateEventArgs args = new OdometryUpdateEventArgs( Int32.Parse(parameters [1]), Int32.Parse(parameters [2]), Double.Parse(parameters [3])); this.OnOdometryUpdate(args); break; case 's': string[] stringReadings = message.Split(','); List <double> readings = new List <double> (); // Somewhat inefficient. Start from 1 to skip first character // which is the message header. for (int i = 1; i < stringReadings.Length; i++) { readings.Add(Double.Parse(stringReadings [i])); } readings.Reverse(); ScanEventArgs args2 = new ScanEventArgs(readings); OnScanPerformed(args2); break; default: break; } } catch (TimeoutException ex) { Console.WriteLine(ex.ToString()); } catch (FormatException ex) { Console.WriteLine(ex.ToString()); } catch (IndexOutOfRangeException ex) { Console.WriteLine(ex.ToString()); } catch (ThreadAbortException) { // Ignore it. } catch (Exception ex) { Console.WriteLine(ex.ToString()); } } }
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e) { robot.UpdateOdometry(e); }
/// <summary> /// Updates the odometry. /// </summary> /// <param name="e">E.</param> public void UpdateOdometry(OdometryUpdateEventArgs e) { bool raiseEvent = false; // First update. if (originalRotation == double.MaxValue) { PathPointList.Add(new double[2] { x, y }); originalRotation = e.Theta; raiseEvent = true; } else { // Check if we have changed rotation. heading = e.Theta - originalRotation; double change = e.Theta - originalRotation; // Rotated by more than a degree? if (change >= 0.02 || change <= -0.02) { raiseEvent = true; } if (heading < 0) { heading += Math.PI * 2; } else if (heading > 2 * Math.PI) { heading -= Math.PI * 2; } heading = Math.Round(heading, 2); /* * Calculate the change as follows: * xm = (displacement / sensor cpi) * conversion of inches to meters * ym = (displacement / sensor cpi) * conversion of inches to meters */ double xm = (e.X / mouseCpi) * inchToMeter; double ym = (e.Y / mouseCpi) * inchToMeter; bool hasMoved = CalculateDisplacement(xm, ym); if (hasMoved) { PathPointList.Add(new double[2] { x, y }); } if (!raiseEvent) { raiseEvent = true; } } if (raiseEvent) { RaiseRobotUpdate(); } }