1 package org.ocera.orte.demo;
3 import java.util.concurrent.locks.ReentrantLock;
4 import java.util.concurrent.locks.ReentrantReadWriteLock;
5 import java.util.concurrent.locks.ReentrantReadWriteLock.ReadLock;
6 import java.util.concurrent.locks.ReentrantReadWriteLock.WriteLock;
8 import org.ocera.orte.DomainApp;
9 import org.ocera.orte.Publication;
10 import org.ocera.orte.types.NtpTime;
11 import org.ocera.orte.types.PublProp;
13 public class MotionSpeedPublish implements Runnable {
15 public static final int VMAX = 16000;
16 public static final double r = 0.15;
17 public final float alpha = 0.8f;
19 private short[] speed = new short[2];
20 private boolean isCancelled = true;
21 private float maxRange;
22 private float[] accelData = new float[2];
23 private float[] accelNew = new float[2];
24 private SpeedMotionType speedmsg;
25 private Publication pub;
26 private Thread thread = null;
27 private PublProp publProp;
28 private DomainApp appDomain;
30 private final ReentrantLock dataLock = new ReentrantLock();
31 private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
32 private final ReadLock rcLock = controlRrwl.readLock();
33 private final WriteLock wcLock = controlRrwl.writeLock();
35 public MotionSpeedPublish(float maxRange, DomainApp appDomain) {
36 this.maxRange = maxRange;
37 this.appDomain = appDomain;
39 NtpTime persistence = new NtpTime(3);
42 speedmsg = new SpeedMotionType(appDomain,"motion_speed");
44 publProp = new PublProp(speedmsg.getTopic(),
50 private void calculateSpeed(float[] mAccel) {
52 double angle = 0, length, v, omega;
54 v = (double)mAccel[1]/maxRange;
55 omega = (double)mAccel[0]/maxRange;
56 length = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
58 angle = Math.atan2(mAccel[1], mAccel[0]);
60 omega = Math.cos(angle);
64 speed[0] = (short)(-(v + (v>0 ? -r*omega : r*omega))*VMAX);
65 speed[1] = (short)(-(v - (v>0 ? -r*omega : r*omega))*VMAX);
72 pub = appDomain.createPublication(publProp, speedmsg);
73 thread = new Thread(this);
81 public void cancel() {
92 } catch (InterruptedException e) {
99 public boolean isCancelled() {
116 accelData[0] = alpha * accelData[0] + (1 - alpha) * accelNew[0];
117 accelData[1] = alpha * accelData[1] + (1 - alpha) * accelNew[1];
119 calculateSpeed(accelData);
124 speedmsg.speed = this.speed;
134 } catch (InterruptedException e) {
140 public void setSpeed(float accelX, float accelY) {
141 if (dataLock.tryLock()) {
143 this.accelNew[0] = accelX;
144 this.accelNew[1] = accelY;