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.75f;
19 private short[] speed = new short[2];
20 private boolean isCancelled = true;
21 private float a2sRatio = 9.81f;
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(DomainApp appDomain) {
36 this.appDomain = appDomain;
38 NtpTime persistence = new NtpTime(3);
41 speedmsg = new SpeedMotionType(appDomain,"motion_speed");
43 publProp = new PublProp(speedmsg.getTopic(),
49 private void calculateSpeed(float[] mAccel) {
51 double angle = 0, length, v, omega;
53 v = (double)mAccel[1]/a2sRatio;
54 omega = (double)mAccel[0]/a2sRatio;
55 length = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
57 angle = Math.atan2(mAccel[1], mAccel[0]);
59 omega = Math.cos(angle);
63 speed[0] = (short)(-((v + r*omega))*VMAX);
64 speed[1] = (short)(-(v - r*omega)*VMAX);
71 pub = appDomain.createPublication(publProp, speedmsg);
72 thread = new Thread(this);
80 public void cancel() {
91 } catch (InterruptedException e) {
98 public boolean isCancelled() {
115 accelData[0] = alpha * accelData[0] + (1 - alpha) * accelNew[0];
116 accelData[1] = alpha * accelData[1] + (1 - alpha) * accelNew[1];
118 calculateSpeed(accelData);
123 speedmsg.speed = this.speed;
133 } catch (InterruptedException e) {
139 public void setSpeed(float accelX, float accelY) {
140 if (dataLock.tryLock()) {
142 this.accelNew[0] = accelX;
143 this.accelNew[1] = accelY;