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 6d6938f4986abc4bec4d9e1051afaf55a9b4981a..f5d8b0f9149f1df702b7ba90e13fe543547dbfb4 100644 (file)
--- 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 dbe102131f54b01a0b4a3d448194e1217e530307..a8f750c270882c9f6993195ffb430c79a0835a62 100644 (file)
--- 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();