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

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