public static final int ic_launcher=0x7f020000;
}
public static final class id {
- public static final int button1=0x7f070000;
- public static final int button2=0x7f070001;
- public static final int button3=0x7f070002;
- public static final int button4=0x7f070003;
- public static final int menu_settings=0x7f070006;
- public static final int scrollView1=0x7f070004;
- public static final int textView1=0x7f070005;
+ public static final int exit=0x7f070005;
+ public static final int hokuyo=0x7f070002;
+ public static final int hokuyo_view=0x7f070000;
+ public static final int lift=0x7f070003;
+ public static final int manget=0x7f070004;
+ public static final int speed=0x7f070001;
}
public static final class layout {
- public static final int activity_main=0x7f030000;
+ public static final int hokuyo_view=0x7f030000;
}
public static final class menu {
public static final int activity_main=0x7f060000;
}
public static final class string {
public static final int app_name=0x7f040000;
- public static final int body1=0x7f040005;
- public static final int button1_tab1=0x7f040002;
- public static final int button1_tab2=0x7f040004;
- public static final int button2_tab1=0x7f040003;
- public static final int button2_tab2=0x7f040006;
- public static final int menu_settings=0x7f040001;
}
public static final class style {
/**
+++ /dev/null
-<?xml version="1.0" encoding="utf-8"?>
-<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
- android:layout_width="fill_parent"
- android:layout_height="fill_parent"
- android:orientation="vertical">
-
- <LinearLayout
- android:layout_width="match_parent"
- android:layout_height="wrap_content" >
-
- <Button
- android:id="@+id/button1"
- android:layout_width="wrap_content"
- android:layout_height="wrap_content"
- android:text="@string/button1_tab1" />
-
- <Button
- android:id="@+id/button2"
- android:layout_width="wrap_content"
- android:layout_height="wrap_content"
- android:text="@string/button2_tab1" />
-
-
-
- </LinearLayout>
-
- <LinearLayout
- android:layout_width="match_parent"
- android:layout_height="wrap_content" >
-
- <Button
- android:id="@+id/button3"
- android:layout_width="wrap_content"
- android:layout_height="wrap_content"
- android:text="@string/button1_tab2" />
-
- <Button
- android:id="@+id/button4"
- android:layout_width="wrap_content"
- android:layout_height="wrap_content"
- android:text="@string/button2_tab2" />
-
- </LinearLayout>
-
- <ScrollView
- android:id="@+id/scrollView1"
- android:layout_width="match_parent"
- android:layout_height="wrap_content" >
-
-
- <TextView
- android:id="@+id/textView1"
- android:layout_width="wrap_content"
- android:layout_height="wrap_content"
- android:text="@string/body1" />
-
-
- </ScrollView>
-
-</LinearLayout>
\ No newline at end of file
--- /dev/null
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+ android:layout_width="fill_parent"
+ android:layout_height="fill_parent"
+ android:orientation="vertical" >
+
+ <org.ocera.orte.demo.HokuyoView
+ android:id="@+id/hokuyo_view"
+ android:layout_width="fill_parent"
+ android:layout_height="fill_parent"/>
+
+</LinearLayout>
<menu xmlns:android="http://schemas.android.com/apk/res/android" >
-
- <item
- android:id="@+id/menu_settings"
- android:orderInCategory="100"
+ <item android:id="@+id/speed"
+ android:title="Start speed"
+ android:showAsAction="never"
+ android:orderInCategory="1">
+ </item>
+
+ <item android:id="@+id/hokuyo"
+ android:title="Start hokuyo"
android:showAsAction="never"
- android:title="@string/menu_settings"/>
+ android:orderInCategory="2">
+ </item>
+
+ <item android:id="@+id/lift"
+ android:title="Lift up"
+ android:orderInCategory="3"
+ android:showAsAction="never">
+ </item>
+
+ <item android:id="@+id/manget"
+ android:title="Magnet on"
+ android:orderInCategory="4"
+ android:showAsAction="never">
+ </item>
+ <item android:id="@+id/exit"
+ android:title="Exit"
+ android:orderInCategory="5"
+ android:showAsAction="never">
+ </item>
</menu>
\ No newline at end of file
<resources>
<string name="app_name">ORTE Demo</string>
- <string name="menu_settings">Settings</string>
- <string name="button1_tab1">Run manager</string>
- <string name="button2_tab1">Stop manager</string>
- <string name="button1_tab2">Run publisher</string>
- <string name="body1"></string>
- <string name="button2_tab2">Stop publisher</string>
</resources>
\ No newline at end of file
+++ /dev/null
-package org.ocera.orte.demo;
-
-/* HelloMsg.java */
-
-/* ********************************************************* *
- * *
- * Will be Generated by IDL *
- * *
- * ********************************************************* */
-
-import java.nio.ByteOrder;
-
-import org.ocera.orte.*;
-import org.ocera.orte.types.*;
-
-
-public class HelloMsg extends MessageData
-{
- private int counter = 0;
- public int count_read = 0; // pro testovaci ucely!!
-
-/* ************************************** *
- * data to send *
- * ************************************** */
-
- public short left;
- public short right;
-
-/* ************************************** */
-
-
- /* constructor */
- public HelloMsg(DomainApp domainApp, String newTopic)
- {
- super();
- buffer.order(ByteOrder.LITTLE_ENDIAN);
-
- // typeName registration
- if(counter == 0)
- {
- boolean b;
-
- counter++;
- this.setTopic(newTopic); // set the topic of a publication
- // register new data typeName
- b = domainApp.regNewDataType("motion_speed",getMaxDataLength());
- if (b == false)
- {
- System.out.println(":j!: cannot register data type!");
- }
-
- }
- // init
- left = 0;
- right = 0;
- //System.out.println(":j: instance of HelloMsg created..");
- }
-
-
- /**
- * Serializetaion method. Put all of data fields and write them into a buffer.
- * @return True - if write successful, False - write failed (IndexOutOfBoundsException or ReadOnlyBufferException).
- */
- /* TODO osetrit vyjimky - mozna pak bude vracet typ boolean
- * IndexOutOfBoundsException - If index is negative or not smaller than the buffer's limit
- * ReadOnlyBufferException - If this buffer is read-only
- */
- public void write()
- {
- /* TODO - control setting fields !!*/
- //
- buffer.rewind(); // set offset to 0
- buffer.putShort(this.left);
- buffer.putShort(this.right);
- }
-
- /**
- * Serializetaion method. Put all of data fields and write them into a buffer.
- * @return True - if write successful, False - write failed (IndexOutOfBoundsException or ReadOnlyBufferException).
- */
- /* TODO 'BufferUnderflowException' - osetrit vyjimky - mozna pak bude vracet typ boolean
-
- */
- public void read()
- {
- buffer.rewind(); // set offset to 0
- this.left = buffer.getShort();
- this.right = buffer.getShort();
- }
-
- /**
- * Return maximum buffer's length according to its data fields.
- * @return Length of buffer.
- **/
- public int getMaxDataLength()
- {
- int len=0;
- len += ORTEConstant.SHORT_FIELD_SIZE;
- len += ORTEConstant.SHORT_FIELD_SIZE;
- return len;
- }
-
- public String toString()
- {
- String data = new String();
-
- data += (" left = " + left);
- data += (" right = " + right);
- return data;
- }
-
-}
\ No newline at end of file
--- /dev/null
+package org.ocera.orte.demo;
+
+import org.ocera.orte.DomainApp;
+import org.ocera.orte.Subscription;
+import org.ocera.orte.SubscriptionCallback;
+import org.ocera.orte.types.MessageData;
+import org.ocera.orte.types.NtpTime;
+import org.ocera.orte.types.RecvInfo;
+import org.ocera.orte.types.SubsProp;
+
+public class HokuyoScanSubscribe extends SubscriptionCallback{
+
+ public final static int IMMEDIATE = 0x02;
+ public final static int BEST_EFFORTS = 0x01;
+
+ private Subscription sub;
+ private HokuyoView view;
+ private HokuyoScanType hokuyomsg;
+ private DomainApp domainApp;
+ private SubsProp subProps;
+ private boolean isCancelled = true;
+
+ public HokuyoScanSubscribe(DomainApp domainApp, HokuyoView view) {
+ this.view = view;
+ this.domainApp = domainApp;
+
+ NtpTime deadline = new NtpTime(10);
+ NtpTime minSeparation = new NtpTime(0);
+
+ hokuyomsg = new HokuyoScanType(domainApp, "hokuyo_scan");
+
+ subProps = new SubsProp(hokuyomsg.getTopic(),
+ "hokuyo_scan",
+ minSeparation,
+ deadline,
+ IMMEDIATE,
+ BEST_EFFORTS,
+ 0);
+ }
+
+ public void start() {
+ isCancelled = false;
+ sub = domainApp.createSubscription(subProps, hokuyomsg, this);
+ }
+
+ public void cancel() {
+ sub.destroy();
+ isCancelled = true;
+ }
+
+ public boolean isCancelled() {
+ return isCancelled;
+ }
+
+ public void callback(RecvInfo info, MessageData msg) {
+ view.setData(((HokuyoScanType)msg).hokuyo);
+ }
+}
\ No newline at end of file
--- /dev/null
+package org.ocera.orte.demo;
+
+import org.ocera.orte.DomainApp;
+import org.ocera.orte.types.MessageData;
+import org.ocera.orte.types.ORTEConstant;
+
+public class HokuyoScanType extends MessageData {
+
+ private int counter = 0;
+
+ public int[] hokuyo = new int[681];
+
+ public HokuyoScanType(DomainApp domainApp, String newTopic) {
+ super();
+ if(counter == 0) {
+ boolean b;
+ counter++;
+ this.setTopic(newTopic);
+ b = domainApp.regNewDataType("hokuyo_scan",getMaxDataLength());
+ if (b == false) {
+ System.out.println(":j!: cannot register data type!");
+ }
+ }
+ }
+
+ @Override
+ public void read() {
+ buffer.rewind();
+ for (int i = 0; i < hokuyo.length; i++) {
+ this.hokuyo[i] = buffer.getShort() & 0xffff;
+ }
+ }
+
+ @Override
+ public void write() {}
+
+ @Override
+ public int getMaxDataLength() {
+ return 681 * ORTEConstant.SHORT_FIELD_SIZE;
+ }
+
+ public String toString() {
+ String data = new String();
+ for(int i = 0; i < hokuyo.length; i++) {
+ data += i + ": " + hokuyo[i] + ", ";
+ }
+ data = data.substring(0, data.length()-2);
+ return data;
+ }
+}
--- /dev/null
+package org.ocera.orte.demo;
+
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.Path;
+import android.graphics.Paint.Style;
+import android.util.AttributeSet;
+import android.view.View;
+
+public class HokuyoView extends View {
+
+ public static final double HOKUYO_START_ANGLE = 239.77/2;
+ public static final int HOKUYO_SPLIT_DIVISION = 1024;
+ public static final int HOKUYO_ORIENTATION = 1;
+ public static final double HOKUYO_RANGE_ANGLE_LEFT = 70.0;
+ public static final double HOKUYO_RANGE_ANGLE_RIGHT = 70.0;
+
+ private int[] data = new int[681];
+ private Paint paint = new Paint();
+ private Path path = new Path();
+ private boolean isRunning = false;
+
+ public HokuyoView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+ }
+
+ @Override
+ protected void onDraw(Canvas canvas) {
+ if (isRunning) {
+ paint.setStyle(Style.STROKE);
+ paint.setStrokeWidth(3);
+ paint.setColor(Color.BLACK);
+ paint.setAntiAlias(true);
+
+ path.reset();
+ path.moveTo(getWidth()/2, getHeight());
+ for(int i = 0; i < data.length; i++) {
+ double ang = HOKUYO_INDEX_TO_RAD(i);
+ if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*Math.PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*Math.PI)))) {
+ continue;
+ }
+
+ data[i] /= 10;
+ int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
+ int y = getHeight() -(int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
+ path.lineTo(x, y);
+ }
+ path.close();
+
+ canvas.drawPath(path, paint);
+ }
+ }
+
+ public void run(boolean run) {
+ isRunning = run;
+ }
+
+ public void setData(int[] data) {
+ this.data = data;
+ postInvalidate();
+ }
+
+ public static double HOKUYO_INDEX_TO_DEG(int index) {
+ return ((HOKUYO_START_ANGLE-index*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION);
+ }
+
+ public static double HOKUYO_INDEX_TO_RAD(int index) {
+ return (HOKUYO_INDEX_TO_DEG(index)/180.0*Math.PI);
+ }
+}
package org.ocera.orte.demo;
import org.ocera.orte.DomainApp;
-import org.ocera.orte.JOrte;
import org.ocera.orte.Manager;
-import org.ocera.orte.Publication;
import org.ocera.orte.types.DomainProp;
-import org.ocera.orte.types.NtpTime;
import org.ocera.orte.types.ORTEConstant;
-import org.ocera.orte.types.PublProp;
import android.app.Activity;
import android.content.Context;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
-import android.os.AsyncTask;
import android.os.Bundle;
import android.os.PowerManager;
import android.os.PowerManager.WakeLock;
-import android.view.View;
-import android.widget.Button;
-import android.widget.TextView;
+import android.view.Menu;
+import android.view.MenuItem;
public class MainActivity extends Activity {
- private Button start_mgr = null;
- private Button stop_mgr = null;
- private Button start_pub = null;
- private Button stop_pub = null;
- private TextView textview = null;
-
private Manager manager = null;
- private AsyncTask<Void, Short, Void> publisher = null;
-
+ private String[] mgrs = {"192.168.1.5","192.168.1.8"};
+ private MotionSpeedPublish motion_speed = null;
+ private HokuyoScanSubscribe hokuyo_scan = null;
private SensorManager mSensorManager = null;
private Sensor mGravity = null;
private SensorEventListener accel = null;
- private float accelData[] = new float[2];
- private Object lock = new Object();
-
private PowerManager mPowerManager = null;
private WakeLock mWakeLock = null;
+ private DomainApp appDomain = null;
+ private HokuyoView hokuyo_view = null;
+ private MenuItem speed_item = null;
+ private MenuItem hokuyo_item = null;
static {
System.loadLibrary("jorte");
}
-
- private class ComputeOnBackground extends AsyncTask<Void, Short, Void> {
-
- public static final int VMAX = 16000;
-
- private short[] calculateSpeed(float[] mAccel) {
- short[] speed = new short[2];
-
- double r=0.15;
- double angle = 0, lenght, v, omega;
-
- v = (double)mAccel[1]/mGravity.getMaximumRange();
- omega = (double)mAccel[0]/mGravity.getMaximumRange();
- lenght = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
- /* if length is more than 1 is used unit circle */
- if (lenght >= 1) {
- /* computing new speed and omega */
- angle = Math.atan2(mAccel[1], mAccel[0]);
- v = Math.sin(angle);
- omega = Math.cos(angle);
- }
- omega *= 2;
-
- speed[0] = (short)(-((v + r*omega))*VMAX);
- speed[1] = (short)(-(v - r*omega)*VMAX);
-
- return speed;
- }
-
- @Override
- protected Void doInBackground(Void... arg0) {
- NtpTime persistence = new NtpTime(3);
- int strength = 100;
- MyEvents events = new MyEvents();
-
- DomainApp appDomain = new DomainApp(ORTEConstant.ORTE_DEFAULT_DOMAIN,
- DomainProp.defaultPropsCreate(),
- events,
- false);
-
- HelloMsg hellomsg = new HelloMsg(appDomain,"motion_speed");
-
- PublProp publProp = new PublProp(hellomsg.getTopic(),
- "motion_speed",
- persistence,
- strength);
-
- Publication pub = appDomain.createPublication(publProp,
- hellomsg);
-
- while(!isCancelled())
- {
- synchronized(lock) {
- short speed[] = calculateSpeed(accelData);
-
- hellomsg.left = speed[0];
- hellomsg.right = speed[1];
- pub.send(hellomsg);
- publishProgress(speed[0],speed[1]);
- }
- JOrte.sleepMs(100);
- }
-
- pub = null;
- System.gc();
- appDomain = null;
- System.gc();
-
- return null;
- }
-
- @Override
- protected void onProgressUpdate(Short... input) {
- textview.setText("Values: left = " + input[0] + ", right = " + input[1]);
- }
- }
-
- private class HandleAccelerometer implements SensorEventListener {
- @Override
- public void onAccuracyChanged(Sensor sensor, int accuracy) {}
-
- @Override
- public void onSensorChanged(SensorEvent event) {
- if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
- synchronized(lock) {
- accelData[0] = event.values[0];
- accelData[1] = event.values[1];
- }
- }
- }
- }
-
@Override
protected void onResume() {
super.onResume();
-
+
mWakeLock.acquire();
}
protected void onPause() {
super.onPause();
- if (publisher != null) {
- publisher.cancel(false);
- publisher = null;
+ if (motion_speed != null && !motion_speed.isCancelled()) {
+ motion_speed.cancel();
+ mSensorManager.unregisterListener(accel);
+ speed_item.setTitle("Start speed");
}
-
- if (accel != null) {
- mSensorManager.unregisterListener(accel);
- accel = null;
+
+ if (hokuyo_scan != null && !hokuyo_scan.isCancelled()) {
+ hokuyo_view.run(false);
+ hokuyo_scan.cancel();
+ hokuyo_item.setTitle("Start hokuyo");
}
- manager = null;
-
mWakeLock.release();
+ }
+
+ @Override
+ protected void onDestroy() {
+ super.onStop();
+
+ if (appDomain != null) {
+ appDomain.destroy();
+ appDomain = null;
+ }
- System.gc();
+ if (manager != null) {
+ manager.destroy();
+ manager = null;
+ }
}
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
- setContentView(R.layout.activity_main);
+ setContentView(R.layout.hokuyo_view);
- textview = (TextView)findViewById(R.id.textView1);
-
- start_mgr = (Button)findViewById(R.id.button1);
- stop_mgr = (Button)findViewById(R.id.button2);
- stop_mgr.setEnabled(false);
- start_pub = (Button)findViewById(R.id.button3);
- start_pub.setEnabled(false);
- stop_pub = (Button)findViewById(R.id.button4);
- stop_pub.setEnabled(false);
-
mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
mGravity = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
mPowerManager = (PowerManager) getSystemService(Context.POWER_SERVICE);
mWakeLock = mPowerManager.newWakeLock(PowerManager.SCREEN_BRIGHT_WAKE_LOCK, getClass().getName());
+
+ hokuyo_view = (HokuyoView) findViewById(R.id.hokuyo_view);
+
+ manager = new Manager(mgrs);
- start_mgr.setOnClickListener(new View.OnClickListener() {
- String[] mgrs = {"192.168.1.5","192.168.1.8"};
-
- @Override
- public void onClick(View v) {
- start_mgr.setEnabled(false);
- stop_mgr.setEnabled(true);
- start_pub.setEnabled(true);
-
- manager = new Manager(mgrs);
- }
- });
+ MyEvents events = new MyEvents();
+ appDomain = new DomainApp(ORTEConstant.ORTE_DEFAULT_DOMAIN,
+ DomainProp.defaultPropsCreate(),
+ events,
+ false);
+ }
+
+ @Override
+ public boolean onCreateOptionsMenu(Menu menu) {
+ getMenuInflater().inflate(R.menu.activity_main, menu);
- stop_mgr.setOnClickListener(new View.OnClickListener() {
-
- @Override
- public void onClick(View v) {
- start_mgr.setEnabled(true);
- stop_mgr.setEnabled(false);
- start_pub.setEnabled(false);
- stop_pub.setEnabled(false);
-
- manager = null;
- System.gc();
- }
- });
+ return true;
+ }
+
+ @Override
+ public boolean onOptionsItemSelected (MenuItem item) {
+ System.out.println(item.getTitle());
- start_pub.setOnClickListener(new View.OnClickListener() {
+ if(item.getTitle().equals("Start speed")) {
+ accel = new HandleAccelerometer();
+ mSensorManager.registerListener(accel, mGravity, SensorManager.SENSOR_DELAY_UI);
+ if (motion_speed == null)
+ motion_speed = new MotionSpeedPublish(mGravity.getMaximumRange(),appDomain);
+ motion_speed.start();
+ speed_item = item;
+ item.setTitle("Stop speed");
+ }
+ else if (item.getTitle().equals("Stop speed")) {
+ mSensorManager.unregisterListener(accel);
+ motion_speed.cancel();
+ item.setTitle("Start speed");
+ }
+ else if (item.getTitle().equals("Start hokuyo")) {
+ if (hokuyo_scan == null)
+ hokuyo_scan = new HokuyoScanSubscribe(appDomain, hokuyo_view);
+ hokuyo_scan.start();
+ hokuyo_view.run(true);
+ hokuyo_item = item;
+ item.setTitle("Stop hokuyo");
+ }
+ else if (item.getTitle().equals("Stop hokuyo")) {
+ hokuyo_view.run(false);
+ hokuyo_scan.cancel();
+ item.setTitle("Start hokuyo");
+ }
+ else if (item.getTitle().equals("Lift up")) {
- @Override
- public void onClick(View v) {
- start_pub.setEnabled(false);
- stop_pub.setEnabled(true);
-
- accel = new HandleAccelerometer();
- mSensorManager.registerListener(accel, mGravity, SensorManager.SENSOR_DELAY_UI);
- publisher = new ComputeOnBackground().execute();
- }
- });
-
- stop_pub.setOnClickListener(new View.OnClickListener() {
+ }
+ else if (item.getTitle().equals("Lift down")) {
+
+ }
+ else if (item.getTitle().equals("Magnet on")) {
+
+ }
+ else if (item.getTitle().equals("Magnet off")) {
- @Override
- public void onClick(View v) {
- start_pub.setEnabled(true);
- stop_pub.setEnabled(false);
+ }
+ else if (item.getTitle().equals("Exit")) {
+ finish();
+ }
+
+ return true;
+ }
+
+ private class HandleAccelerometer implements SensorEventListener {
+
+ @Override
+ public void onAccuracyChanged(Sensor sensor, int accuracy) {}
- publisher.cancel(false);
- mSensorManager.unregisterListener(accel);
- publisher = null;
- accel = null;
- System.gc();
- }
- });
+ @Override
+ public void onSensorChanged(SensorEvent event) {
+ if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
+ if (motion_speed != null)
+ motion_speed.setSpeed(event.values[0], event.values[1]);
+ }
+ }
}
}
\ No newline at end of file
--- /dev/null
+package org.ocera.orte.demo;
+
+import org.ocera.orte.DomainApp;
+import org.ocera.orte.JOrte;
+import org.ocera.orte.Publication;
+import org.ocera.orte.types.NtpTime;
+import org.ocera.orte.types.PublProp;
+
+public class MotionSpeedPublish implements Runnable {
+
+ public static final int VMAX = 16000;
+ public static final double r = 0.15;
+
+ private short[] speed = new short[2];
+ private boolean isCancelled = false;
+ private float maxRange;
+ private float[] accelData = new float[2];
+ private SpeedMotionType speedmsg;
+ private Publication pub;
+ private Object lock = new Object();
+ private Thread thread = null;
+ private PublProp publProp;
+ private DomainApp appDomain;
+
+ public MotionSpeedPublish(float maxRange, DomainApp appDomain) {
+ this.maxRange = maxRange;
+ this.appDomain = appDomain;
+
+ NtpTime persistence = new NtpTime(3);
+ int strength = 100;
+
+ speedmsg = new SpeedMotionType(appDomain,"motion_speed");
+
+ publProp = new PublProp(speedmsg.getTopic(),
+ "motion_speed",
+ persistence,
+ strength);
+
+
+ }
+
+ public void start() {
+ isCancelled = false;
+ pub = appDomain.createPublication(publProp, speedmsg);
+ thread = new Thread(this);
+ thread.start();
+ }
+
+ private void calculateSpeed(float[] mAccel) {
+
+ double angle = 0, lenght, v, omega;
+
+ v = (double)mAccel[1]/maxRange;
+ omega = (double)mAccel[0]/maxRange;
+ lenght = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
+ if (lenght >= 1) {
+ angle = Math.atan2(mAccel[1], mAccel[0]);
+ v = Math.sin(angle);
+ omega = Math.cos(angle);
+ }
+ omega *= 2;
+
+ speed[0] = (short)(-((v + r*omega))*VMAX);
+ speed[1] = (short)(-(v - r*omega)*VMAX);
+ }
+
+ public void cancel() {
+ isCancelled = true;
+ try {
+ thread.join();
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ pub.destroy();
+ }
+
+ public boolean isCancelled() {
+ return isCancelled;
+ }
+
+ @Override
+ public void run() {
+ while(!isCancelled) {
+ synchronized(lock) {
+ calculateSpeed(accelData);
+ speedmsg.speed = this.speed;
+
+ pub.send(speedmsg);
+ }
+
+ JOrte.sleepMs(100);
+ }
+ }
+
+ public void setSpeed(float accelX, float accelY) {
+ synchronized(lock) {
+ this.accelData[0] = accelX;
+ this.accelData[1] = accelY;
+ }
+ }
+
+}
\ No newline at end of file
super();
}
+ @Override
public void onRegFail()
{
System.out.println(":j: *************************************************");
System.out.println(":j: *************************************************");
}
+ @Override
public void onMgrNew(AppInfo appInfo)
{
System.out.println(":j: *************************************************");
System.out.println(":j: *************************************************");
}
+ @Override
public void onMgrDelete(AppInfo appInfo)
{
System.out.println(":j: *************************************************");
System.out.println(":j: *************************************************");
}
+ @Override
public void onAppRemoteNew(AppInfo appInfo)
{
System.out.println(":j: *************************************************");
appInfo.printWithLegend();
}
+ @Override
public void onAppDelete(AppInfo appInfo)
{
System.out.println(":j: *************************************************");
appInfo.printWithLegend();
}
+ @Override
public void onPubRemoteNew(AppInfo appInfo, PubInfo pubInfo)
{
System.out.println(":j: *************************************************");
pubInfo.printWithLegend();
}
+ @Override
public void onPubRemoteChanged(AppInfo appInfo, PubInfo pubInfo)
{
System.out.println(":j: *************************************************");
pubInfo.printWithLegend();
}
+ @Override
public void onPubDelete(AppInfo appInfo, PubInfo pubInfo)
{
System.out.println(":j: *************************************************");
pubInfo.printWithLegend();
}
-
+ @Override
public void onSubRemoteNew(AppInfo appInfo, SubInfo subInfo)
{
System.out.println(":j: *************************************************");
subInfo.printWithLegend();
}
+ @Override
public void onSubRemoteChanged(AppInfo appInfo, SubInfo subInfo)
{
System.out.println(":j: *************************************************");
subInfo.printWithLegend();
}
+ @Override
public void onSubDelete(AppInfo appInfo, SubInfo subInfo)
{
System.out.println(":j: *************************************************");
--- /dev/null
+package org.ocera.orte.demo;
+
+import org.ocera.orte.*;
+import org.ocera.orte.types.*;
+
+
+public class SpeedMotionType extends MessageData
+{
+ private int counter = 0;
+
+ public short[] speed = new short[2];
+
+ public SpeedMotionType(DomainApp domainApp, String newTopic) {
+ super();
+ if(counter == 0) {
+ boolean b;
+ counter++;
+ this.setTopic(newTopic);
+ b = domainApp.regNewDataType("motion_speed",getMaxDataLength());
+ if (b == false) {
+ System.out.println(":j!: cannot register data type!");
+ }
+ }
+ }
+
+ @Override
+ public void write()
+ {
+ buffer.rewind();
+ buffer.putShort(this.speed[0]);
+ buffer.putShort(this.speed[1]);
+ }
+
+ @Override
+ public void read() {}
+
+ @Override
+ public int getMaxDataLength()
+ {
+ int len=0;
+ len += ORTEConstant.SHORT_FIELD_SIZE;
+ len += ORTEConstant.SHORT_FIELD_SIZE;
+ return len;
+ }
+
+ public String toString()
+ {
+ String data = new String();
+
+ data += (" left = " + speed[0]);
+ data += (" right = " + speed[1]);
+ return data;
+ }
+
+}
\ No newline at end of file
projects / orte.git / commitdiff