From: Martin Vajnar <martin.vajnar@gmail.com>
Date: Sun, 25 Aug 2013 22:08:52 +0000 (+0200)
Subject: ROBOT_DEMO: add basic low-pass filter
X-Git-Tag: v0.3.4~54
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/orte.git/commitdiff_plain/e7d00e55210695d9ed03167a2b84a86c8bab4225?ds=sidebyside

ROBOT_DEMO: add basic low-pass filter

Add basic low-pass filter for motion_speed publisher.

Change CTU logo size.
---

diff --git a/orte/Robot_Demo/res/layout/hokuyo_view.xml b/orte/Robot_Demo/res/layout/hokuyo_view.xml
index 6d6938f..f5d8b0f 100644
--- a/orte/Robot_Demo/res/layout/hokuyo_view.xml
+++ b/orte/Robot_Demo/res/layout/hokuyo_view.xml
@@ -12,7 +12,7 @@
         android:layout_alignParentRight="true"
         android:layout_alignParentTop="true"
         android:adjustViewBounds="true"
-        android:maxHeight="100dp"
+        android:maxHeight="70dp"
         android:scaleType="fitXY"
         android:src="@drawable/logo_cvut" />
     
diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
index dbe1021..a8f750c 100644
--- a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
+++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
@@ -14,11 +14,13 @@ public class MotionSpeedPublish implements Runnable {
 	
 	public static final int VMAX = 16000;
 	public static final double r = 0.15;
+	public final float alpha = 0.8f;
 	
-	private short[] speed = new short[2];
+	private short[] speed = new short[2]; 
 	private boolean isCancelled = true;
 	private float maxRange;
 	private float[] accelData = new float[2];
+	private float[] accelNew = new float[2];
 	private SpeedMotionType speedmsg;
 	private Publication pub;
 	private Thread thread = null;
@@ -111,6 +113,9 @@ public class MotionSpeedPublish implements Runnable {
 			    if (!isCancelled) {
 			      dataLock.lock();
 			      try {
+			    	  accelData[0] = alpha * accelData[0] + (1 - alpha) * accelNew[0];
+			    	  accelData[1] = alpha * accelData[1] + (1 - alpha) * accelNew[1];
+
 			    	  calculateSpeed(accelData);
 			      }
 			      finally {
@@ -135,8 +140,8 @@ public class MotionSpeedPublish implements Runnable {
 	public void setSpeed(float accelX, float accelY) {
 		if (dataLock.tryLock()) {
 			try {
-				this.accelData[0] = accelX;
-				this.accelData[1] = accelY;
+				this.accelNew[0] = accelX;
+				this.accelNew[1] = accelY;
 			}
 			finally {
 				dataLock.unlock();