private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e) { Gdk.Threads.Enter (); try { robot.UpdateOdometry (e); } finally { Gdk.Threads.Leave (); } }
/// <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 (); }
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e) { Console.WriteLine(e.ToString()); }
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 ()); } } }
protected virtual void OnOdometryUpdate(OdometryUpdateEventArgs e) { if (OdometryUpdated != null) { OdometryUpdated (this, e); } }
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e) { robot.UpdateOdometry (e); }