orte/contrib/Robot_Demo/src/org/ocera/orte/demo/Publishers/MotionSpeedPublish.java

   1 package org.ocera.orte.demo.Publishers;
   2
   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;
   7
   8 import org.ocera.orte.DomainApp;
   9 import org.ocera.orte.Publication;
  10 import org.ocera.orte.demo.DataTypes.SpeedMotionType;
  11 import org.ocera.orte.types.NtpTime;
  12 import org.ocera.orte.types.PublProp;
  13
  14 public class MotionSpeedPublish implements Runnable {
  15
  16         public static final int VMAX = 16000;
  17         public static final double r = 0.15;
  18         public final float alpha = 0.75f;
  19
  20         private short[] speed = new short[2];
  21         private boolean isCancelled = true;
  22         private float a2sRatio = 9.81f;
  23         private float[] accelData = new float[2];
  24         private float[] accelNew = new float[2];
  25         private SpeedMotionType speedmsg;
  26         private Publication pub;
  27         private Thread thread = null;
  28         private PublProp publProp;
  29         private DomainApp appDomain;
  30
  31         private final ReentrantLock dataLock = new ReentrantLock();
  32         private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
  33         private final ReadLock rcLock = controlRrwl.readLock();
  34         private final WriteLock wcLock = controlRrwl.writeLock();
  35
  36         public MotionSpeedPublish(DomainApp appDomain) {
  37                 this.appDomain = appDomain;
  38
  39             NtpTime persistence = new NtpTime(3);
  40             int strength = 100;
  41
  42             speedmsg = new SpeedMotionType(appDomain,"motion_speed");
  43
  44             publProp = new PublProp(speedmsg.getTopic(),
  45                                                                          "motion_speed",
  46                                                                          persistence,
  47                                                                          strength);
  48         }
  49
  50         private void calculateSpeed(float[] mAccel) {
  51
  52                 double angle = 0, length, v, omega;
  53
  54                 v = (double)mAccel[1]/a2sRatio;
  55                 omega = (double)mAccel[0]/a2sRatio;
  56                 length = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
  57                 if (length >= 1) {
  58                         angle = Math.atan2(mAccel[1], mAccel[0]);
  59                         v = Math.sin(angle);
  60                         omega = Math.cos(angle);
  61                 }
  62                 omega *= 2;
  63
  64                 speed[0] = (short)(-((v + r*omega))*VMAX);
  65                 speed[1] = (short)(-(v - r*omega)*VMAX);
  66         }
  67
  68         public void start() {
  69                 wcLock.lock();
  70                 try {
  71                         isCancelled = false;
  72                     pub = appDomain.createPublication(publProp, speedmsg);
  73                     thread = new Thread(this);
  74                     thread.start();
  75                 }
  76                 finally {
  77                         wcLock.unlock();
  78                 }
  79         }
  80
  81         public void cancel() {
  82                 wcLock.lock();
  83                 try {
  84                         isCancelled = true;
  85                         rcLock.lock();
  86                 } finally {
  87                         wcLock.unlock();
  88                 }
  89                 try {
  90                         thread.join();
  91                     pub.destroy();
  92                 } catch (InterruptedException e) {
  93                         e.printStackTrace();
  94                 } finally {
  95                         rcLock.unlock();
  96                 }
  97         }
  98
  99         public boolean isCancelled() {
 100                 rcLock.lock();
 101                 try {
 102                         return isCancelled;
 103                 } finally {
 104                         rcLock.unlock();
 105                 }
 106         }
 107
 108         @Override
 109         public void run() {
 110                 while(true) {
 111                         rcLock.lock();
 112                         try {
 113                             if (!isCancelled) {
 114                               dataLock.lock();
 115                               try {
 116                                   accelData[0] = alpha * accelData[0] + (1 - alpha) * accelNew[0];
 117                                   accelData[1] = alpha * accelData[1] + (1 - alpha) * accelNew[1];
 118
 119                                   calculateSpeed(accelData);
 120                               }
 121                               finally {
 122                                   dataLock.unlock();
 123                               }
 124                           speedmsg.speed = this.speed;
 125                               pub.send(speedmsg);
 126                                 }
 127                             else
 128                                 break;
 129                         } finally {
 130                                 rcLock.unlock();
 131                         }
 132                         try {
 133                                 Thread.sleep(100);
 134                         } catch (InterruptedException e) {
 135                                 e.printStackTrace();
 136                         }
 137                 }
 138         }
 139
 140         public void setSpeed(float accelX, float accelY) {
 141                 if (dataLock.tryLock()) {
 142                         try {
 143                                 this.accelNew[0] = accelX;
 144                                 this.accelNew[1] = accelY;
 145                         }
 146                         finally {
 147                                 dataLock.unlock();
 148                         }
 149                 }
 150         }
 151 }