Don't like ads? PRO users don't see any ads ;-)
Public Pastes
Guest

Untitled

By: a guest on May 8th, 2012  |  syntax: None  |  size: 5.67 KB  |  hits: 22  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. import java.io.BufferedReader;
  2. import java.io.DataInputStream;
  3. import java.io.FileInputStream;
  4. import java.io.InputStreamReader;
  5. import java.util.ArrayList;
  6.  
  7. import javaclient3.PlayerClient;
  8. import javaclient3.PlayerDevice;
  9. import javaclient3.Position2DInterface;
  10. import javaclient3.RangerInterface;
  11. import javaclient3.SonarInterface;
  12. import javaclient3.structures.PlayerConstants;
  13.  
  14.  
  15. public class SafeGTo {
  16.         static boolean done=false;
  17.     @SuppressWarnings("deprecation")
  18.         public static void main(String[] args) {
  19.     ArrayList<ArrayList<Double>> points =new ArrayList<ArrayList<Double>>();
  20.         PlayerClient pc;
  21.        
  22.        
  23.          pc = new PlayerClient("lewis.rit.edu",6665);
  24.                        
  25.                            
  26.                     try{
  27.                           FileInputStream fstream = new FileInputStream(args[0]);
  28.                           DataInputStream in = new DataInputStream(fstream);
  29.                           BufferedReader br = new BufferedReader(new InputStreamReader(in));
  30.                           String strLine;
  31.                          
  32.                           int n=0;
  33.                           ArrayList<Double> temp= new ArrayList<Double>();
  34.                           while ((strLine = br.readLine()) != null)   {
  35.                           for(int i=0;i<strLine.split(" ").length;i++){
  36.                                  
  37.                                   temp.add(Double.valueOf(strLine.split(" ")[i]));
  38.                                
  39.                                
  40.                           }
  41.                           System.out.println(temp.toString());
  42.  
  43.                           ArrayList<Double> element = new ArrayList<Double>();
  44.                           element.addAll(temp);
  45.                           points.add(element);
  46.                           temp.clear();
  47.                
  48.                           n++;
  49.                           }
  50.                          
  51.                           in.close();
  52.                          
  53.                             }catch (Exception e){//Catch exception if any
  54.                           System.err.println("Error: " + e.getMessage());
  55.                           e.printStackTrace();}
  56.                          
  57.        
  58.         Position2DInterface pos = pc.requestInterfacePosition2D(0,PlayerConstants.PLAYER_OPEN_MODE);
  59.         pos.setMotorPower(1);
  60.         SonarInterface ranger =pc.requestInterfaceSonar(0,PlayerConstants.PLAYER_OPEN_MODE);
  61.         while(pc.requestInterfaceSonar(0,PlayerConstants.PLAYER_OPEN_MODE)==null){
  62.         ranger = pc.requestInterfaceSonar(0,PlayerConstants.PLAYER_OPEN_MODE);
  63.         }
  64.         double turnrate = 0;
  65.         double fwd = .0;
  66.        
  67.         double speeds[]={};
  68.  
  69.         float[] ranges={};
  70.        
  71.         pc.readAll();
  72.         for(int n=0;n<points.size();n++){
  73.                 System.out.println("point X: " + points.get(n).get(0) + " point Y: " + points.get(n).get(1));
  74.                 done=false;
  75.                 while(!done){
  76.                         double sumx = 0;
  77.                         double sumy = 0;
  78.                         double angle;
  79.                         double oXY[]={0.0,0.0};
  80.                         double x;
  81.                         double y;
  82.                        
  83.                         double goalvect[]={0.0,0.0};
  84.                         double objvect[]={0.0,0.0};
  85.                         ArrayList<Double> finalvect=new ArrayList<Double>();
  86.                        
  87.                         pc.readAll();
  88.                         //!pos.isDataReady()
  89.                         if (!ranger.isDataReady() || !pos.isDataReady()){
  90.                                 continue;
  91.                         }              
  92.                        
  93.                 ranges = ranger.getData().getRanges();
  94.                 for(int i=0;i<=7;i++){
  95.                
  96.                         if(ranges[i]> .7){
  97.                                 continue;
  98.                         }else{
  99.                                  System.out.println("I have detected an object!");
  100.                                  angle =get_range_angle(i,ranges);
  101.                                  oXY=get_XY(ranges[i],angle);
  102.                                  x=(oXY[0]/(Math.pow(oXY[0], 2))+(Math.pow(oXY[1], 2)));
  103.                                  y=(oXY[1]/(Math.pow(oXY[0], 2))+(Math.pow(oXY[1], 2)));
  104.        
  105.                                  sumx+=x;
  106.                                  sumy+=y;
  107.                         }
  108.                 }
  109.  
  110.                 objvect[0]=sumx;
  111.                 objvect[1]=sumy;
  112.        
  113.                 double d=Math.sqrt(Math.pow(points.get(n).get(0),2)+Math.pow(points.get(n).get(1), 2));
  114.                
  115.                 goalvect[0]=(points.get(n).get(0)-pos.getX())/d;
  116.                 goalvect[1]=(points.get(n).get(1)-pos.getY())/d;
  117.                
  118.                 double newangle=Math.atan2(goalvect[1],goalvect[0])-pos.getYaw();
  119.                
  120.                
  121.                 goalvect[0]=Math.cos(newangle)*1;
  122.                 goalvect[1]=Math.sin(newangle)*1;
  123.                 //System.out.println("goal x local " +goalvect[0] + "goal y local" +  goalvect[1]);
  124.                
  125.                 finalvect.add(0,(.7*goalvect[0])-(.2*objvect[0]));
  126.                 finalvect.add(1,(.7*goalvect[1])-(.2*objvect[1]));
  127.                
  128.                 ArrayList<Double> goal=new ArrayList<Double>();
  129.                 goal=points.get(n);
  130.                 speeds=goTo(finalvect,pos,turnrate,fwd,goal);
  131.                
  132.                 turnrate=speeds[0];
  133.                 fwd=speeds[1];
  134.                
  135.                 pos.setSpeed(fwd,turnrate);
  136.                
  137.        
  138.                
  139.                         }
  140.                
  141.                 }
  142.        
  143.     }
  144.  
  145.     private static double[] get_XY(double d, double angle) {
  146.                 double xy[]={0.0,0.0};
  147.                
  148.                 xy[0]=d*Math.cos(angle);
  149.                 xy[1]=d*Math.sin(angle);
  150.                 return xy;
  151.         }
  152.  
  153.         private static double get_range_angle(int i, float[] ranges) {
  154.                 double angle = 90-(180/8)*i;
  155.                 return (angle*Math.PI/180);
  156.                
  157.         }
  158.        
  159.         public static double [] goTo(ArrayList<Double> points,Position2DInterface pos,double curturn,double curfwd, ArrayList<Double> goal){
  160.        
  161.         double speeds [] = {0.0,0.0};
  162.         double EPSILON = 0.01;
  163.         //double distance=Math.sqrt((points.get(0)-pos.getX())+(points.get(1)-pos.getY()));
  164.        
  165.         double angle=Math.atan2(points.get(1),points.get(0));
  166.        
  167.         //System.out.println("The Angle before: "+Math.abs(angle));
  168.         double diff=angle-pos.getYaw();
  169.         if(angle>Math.PI){
  170.                 angle-=2*Math.PI;
  171.                 }
  172.         else if(angle<-Math.PI){
  173.                 angle+=2*Math.PI;
  174.                 }
  175.         double angle_deg=angle*180/Math.PI;
  176.         double yaw_deg=pos.getYaw()*180/Math.PI;
  177.         double yaw_ang_sub=(angle_deg-yaw_deg);
  178.         System.out.println("yaw_ang_sub " + yaw_ang_sub);
  179.         //System.out.println("The Angle: "+Math.abs(angle));
  180.         if(angle>0){
  181.                 //System.out.println("I feel the need to turn right! "+" the Yaw " + pos.getYaw() + " angle-pos " + (angle-pos.getYaw()));
  182.                 curturn=.2;
  183.                
  184.         }else if(angle<0){
  185.                
  186.                 //System.out.println("I feel the need to turn left! " + " the Yaw " + pos.getYaw() + " angle-pos " + (angle-pos.getYaw()));
  187.                 curturn=-.2;
  188.         }
  189.    
  190.         if(Math.sqrt(((goal.get(0)-pos.getX())+(goal.get(1)-pos.getY()))/10)<0.1  ){
  191.                 //System.out.println("gets here");
  192.                         curfwd=0 ;
  193.                         curturn=0;
  194.                         speeds[0]=curturn;
  195.                 speeds[1]=curfwd;
  196.                 done=true;
  197.                 }else{
  198.                 curfwd=.2;
  199.                         speeds[0]=curturn;
  200.                 speeds[1]=curfwd;
  201.                 }
  202.         speeds[0]=curturn;
  203.         speeds[1]=curfwd;
  204.                 return speeds;
  205.         }
  206. }
create a new version of this paste RAW Paste Data