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

   1 package org.ocera.orte.demo;
   2
   3 import org.ocera.orte.DomainApp;
   4 import org.ocera.orte.JOrte;
   5 import org.ocera.orte.Publication;
   6 import org.ocera.orte.types.NtpTime;
   7 import org.ocera.orte.types.PublProp;
   8
   9 public class MotionSpeedPublish implements Runnable {
  10
  11         public static final int VMAX = 16000;
  12         public static final double r = 0.15;
  13
  14         private short[] speed = new short[2];
  15         private boolean isCancelled = false;
  16         private float maxRange;
  17         private float[] accelData = new float[2];
  18         private SpeedMotionType speedmsg;
  19         private Publication pub;
  20         private Object lock = new Object();
  21         private Thread thread = null;
  22         private PublProp publProp;
  23         private DomainApp appDomain;
  24
  25         public MotionSpeedPublish(float maxRange, DomainApp appDomain) {
  26                 this.maxRange = maxRange;
  27                 this.appDomain = appDomain;
  28
  29             NtpTime persistence = new NtpTime(3);
  30             int strength = 100;
  31
  32             speedmsg = new SpeedMotionType(appDomain,"motion_speed");
  33
  34             publProp = new PublProp(speedmsg.getTopic(),
  35                                                                          "motion_speed",
  36                                                                          persistence,
  37                                                                          strength);
  38
  39
  40         }
  41
  42         public void start() {
  43                 isCancelled = false;
  44             pub = appDomain.createPublication(publProp, speedmsg);
  45             thread = new Thread(this);
  46             thread.start();
  47         }
  48
  49         private void calculateSpeed(float[] mAccel) {
  50
  51                 double angle = 0, lenght, v, omega;
  52
  53                 v = (double)mAccel[1]/maxRange;
  54                 omega = (double)mAccel[0]/maxRange;
  55                 lenght = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
  56                 if (lenght >= 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 + (v>0 ? -r*omega : r*omega))*VMAX);
  64                 speed[1] = (short)(-(v - (v>0 ? -r*omega : r*omega))*VMAX);
  65         }
  66
  67         public void cancel() {
  68                 isCancelled = true;
  69                 try {
  70                         thread.join();
  71                 } catch (InterruptedException e) {
  72                         e.printStackTrace();
  73                 }
  74             pub.destroy();
  75         }
  76
  77         public boolean isCancelled() {
  78                 return isCancelled;
  79         }
  80
  81         @Override
  82         public void run() {
  83             while(!isCancelled) {
  84               synchronized(lock) {
  85                   calculateSpeed(accelData);
  86                   speedmsg.speed = this.speed;
  87
  88                   pub.send(speedmsg);
  89               }
  90
  91           JOrte.sleepMs(100);
  92                 }
  93         }
  94
  95         public void setSpeed(float accelX, float accelY) {
  96                 synchronized(lock) {
  97                         this.accelData[0] = accelX;
  98                         this.accelData[1] = accelY;
  99                 }
 100         }
 101
 102 }