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