API tools faq
paste
Advertisement
Guest User

Roomba robot Java source

a guest
Jul 22nd, 2012
51
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 14.39 KB | None | 0 0
raw download clone embed print report
  1. /* As noted in the video this is just a hacked example of the centralnexus joystick api
  2.  * (http://sourceforge.net/projects/javajoystick/)
  3.  * Server command:
  4.  * ser2net -d -C 1337:raw:0:/dev/ttyACM0
  5.  * Arduino source: http://pastebin.com/Z5dtjkHN
  6.  */
  7.  
  8. package com.centralnexus.test;
  9.  
  10. import java.awt.*;
  11. import java.net.*;
  12. import java.awt.event.*;
  13. import java.io.*;
  14.  
  15. import com.centralnexus.input.*;
  16.  
  17. public class WindowTest
  18.  extends Frame
  19.  implements Runnable, JoystickListener, ActionListener
  20. {
  21.     Socket net;
  22.     PrintStream ps;
  23.     double servo1 = 0;
  24.     double servo2 = 0;
  25.     Joystick joy;
  26.  
  27.     Joystick joy2;
  28.  
  29.     /** polling interval for this joystick */
  30.     private int interval = 50;
  31.  
  32.     Thread thread = new Thread(this);
  33.  
  34.     Label buttonLabel = new Label(),
  35.           button2Label = new Label(),
  36.           deadZoneLabel = new Label(),
  37.           xLabel = new Label(),
  38.           yLabel = new Label(),
  39.           zLabel = new Label(),
  40.           rLabel = new Label(),
  41.           uLabel = new Label(),
  42.           vLabel = new Label(),
  43.           povLabel = new Label();
  44.     Label xyLabel = new Label();
  45.     Label intervalLabel = new Label();
  46.  
  47.     Button addButton = new Button("Add Listener");
  48.     Button removeButton = new Button("Remove Listener");
  49.  
  50.     WindowTest() throws IOException {
  51.         super();
  52.  
  53.         joy = Joystick.createInstance();
  54.         for (int idx = joy.getID() + 1; idx < Joystick.getNumDevices(); idx++) {
  55.             if (Joystick.isPluggedIn(idx)) {
  56.                 joy2 = Joystick.createInstance(idx);
  57.             }
  58.         }
  59.         if (joy2 == null) {
  60.             joy2 = joy;
  61.         }
  62.         doWindowLayout();
  63.         net = new Socket("192.168.100.87",1337);
  64.         ps = new PrintStream(net.getOutputStream());
  65.     }
  66.  
  67.     WindowTest(int joystickID, int joyID2) throws IOException {
  68.         super();
  69.  
  70.         joy = Joystick.createInstance(joystickID);
  71.         joy2 = Joystick.createInstance(joyID2);
  72.         doWindowLayout();
  73.     }
  74.  
  75.     private void doWindowLayout() {
  76.         addWindowListener(new WindowAdapter() {
  77.             public void windowClosing(WindowEvent e) {
  78.                 dispose();
  79.                 System.exit(0);
  80.             }
  81.         });
  82.         setTitle("Joystick Test");
  83.  
  84.         setLayout(new GridLayout(20, 2));
  85.         add(new Label("Number Of Devices: ", Label.RIGHT));
  86.         add(new Label(Integer.toString(Joystick.getNumDevices())));
  87.         add(new Label("Joystick ID: ", Label.RIGHT));
  88.         add(new Label("joy(" + Integer.toString(joy.getID()) + "), joy#2(" + Integer.toString(joy2.getID()) + ")"));
  89.         add(new Label("Description joy#1: ", Label.RIGHT));
  90.         add(new Label(joy.toString()));
  91.         add(new Label("Description joy#2: ", Label.RIGHT));
  92.         add(new Label(joy2.toString()));
  93.         add(new Label("Capabilities:", Label.RIGHT));
  94.         add(new Label("joy(0x" + Integer.toHexString(joy.getCapabilities()) + "), joyEx(0x" + Integer.toHexString(joy2.getCapabilities()) + ")"));
  95.         add(new Label("Axes: ", Label.RIGHT));
  96.         add(new Label("joy(" + Integer.toString(joy.getNumAxes()) + "), joy#2(" + Integer.toString(joy2.getNumAxes()) + ")"));
  97.         add(new Label("Buttons: ", Label.RIGHT));
  98.         add(new Label("joy(" + Integer.toString(joy.getNumButtons()) + "), joy#2(" + Integer.toString(joy2.getNumButtons()) + ")"));
  99.         add(new Label("Dead Zone Size: ", Label.RIGHT));
  100.         add(deadZoneLabel);
  101.         add(new Label("Buttons Pressed: 0x", Label.RIGHT));
  102.         add(buttonLabel);
  103.         add(new Label("X: ", Label.RIGHT));
  104.         add(xLabel);
  105.         add(new Label("Y: ", Label.RIGHT));
  106.         add(yLabel);
  107.         add(new Label("Z: ", Label.RIGHT));
  108.         add(zLabel);
  109.         add(new Label("R: ", Label.RIGHT));
  110.         add(rLabel);
  111.         add(new Label("U: ", Label.RIGHT));
  112.         add(uLabel);
  113.         add(new Label("V: ", Label.RIGHT));
  114.         add(vLabel);
  115.         add(new Label("POV: ", Label.RIGHT));
  116.         add(povLabel);
  117.         add(new Label("Joystick#2(x, y): ", Label.RIGHT));
  118.         add(xyLabel);
  119.         add(new Label("Buttons Pressed: 0x", Label.RIGHT));
  120.         add(button2Label);
  121.         add(new Label("Polling interval: ", Label.RIGHT));
  122.         add(intervalLabel);
  123.         addButton.addActionListener(this);
  124.         removeButton.addActionListener(this);
  125.     }
  126.  
  127.     public void actionPerformed(ActionEvent e) {
  128. //        System.out.println(e);
  129.         if (e.getSource() == addButton) {
  130.             try {
  131.                 net = new Socket("192.168.100.87",1337);
  132.                 ps = new PrintStream(net.getOutputStream());
  133.             } catch (UnknownHostException e1) {
  134.                 // TODO Auto-generated catch block
  135.                 e1.printStackTrace();
  136.             } catch (IOException e1) {
  137.                 // TODO Auto-generated catch block
  138.                 e1.printStackTrace();
  139.             }
  140.         }
  141.         else {
  142.             joy.removeJoystickListener(this);
  143.             joy2.removeJoystickListener(this);
  144.         }
  145.     }
  146.  
  147.     /**
  148.      * This is used by the internal thread.  It creates a lot of String
  149.      * objects, so it uses the garbage collector a lot.  Since this is
  150.      * for testing only, this is not a problem for speed.
  151.      */
  152.     public void run() {
  153.         for (;;) {
  154.             joy.poll();
  155.             joy2.poll();
  156.  
  157.             updateFieldsEx(joy);
  158.             updateFields(joy2);
  159.             try {
  160.                 Thread.sleep(interval);
  161.             } catch(InterruptedException e) {
  162.                 break;
  163.             }
  164.         }
  165.     }
  166.  
  167.     public void joystickAxisChanged(Joystick j) {
  168. //        System.out.println(j.toString());
  169.         if (j == joy) {
  170.             updateFieldsEx(j);
  171.         }
  172.         else {
  173.             updateFields(j);
  174.         }
  175.     }
  176.  
  177.     public void joystickButtonChanged(Joystick j) {
  178. //        System.out.println(j.toString());
  179.         if (j == joy) {
  180.             updateFieldsEx(j);
  181.         }
  182.         else {
  183.             updateFields(j);
  184.         }
  185.     }
  186.  
  187.     public void setPollInterval(int pollMillis) {
  188.         interval = pollMillis;
  189.         joy.setPollInterval(pollMillis);
  190.         joy2.setPollInterval(pollMillis);
  191.         intervalLabel.setText(Integer.toString(interval));
  192.     }
  193.  
  194.     public void updateFields(Joystick joystick) {
  195.         button2Label.setText(Integer.toHexString(joystick.getButtons()));
  196.         xyLabel.setText(joystick.getX() + ", " + joystick.getY());
  197.     }
  198.  
  199.     public void updateFieldsEx(Joystick joystick) {
  200.         buttonLabel.setText(Integer.toHexString(joystick.getButtons()));
  201.         xLabel.setText(Double.toString(joystick.getX()));
  202.         yLabel.setText(Double.toString(joystick.getY()));
  203.         zLabel.setText(Double.toString(joystick.getZ()));
  204.         rLabel.setText(Double.toString(joystick.getR()));
  205.         uLabel.setText(Double.toString(joystick.getU()));
  206.         vLabel.setText(Double.toString(joystick.getV()));
  207.         double joyX = joystick.getX();
  208.         double joyY = joystick.getY();
  209.         double povX = joystick.getU();
  210.         double povY = joystick.getV();
  211.         double povS = joystick.getZ()*10;
  212.         double joyY1 = joyY;
  213.         double joyY2 = joyY;
  214.         double m1 = 0;
  215.         double m2 = 0;
  216.         joyY1 += joyX;
  217.         joyY2 -= joyX;
  218.         if (joyY1 > 0d){
  219.             m1 = joyY1 * 126d;
  220.         } else if (joyY1 < 0d){
  221.             m1 = 128d + (-joyY1) * 126d;
  222.         }  
  223.         if (joyY2 > 0d){
  224.             m2 = joyY2 * 126d;
  225.         } else if (joyY2 < 0d){
  226.             m2 = 128d + (-joyY2) * 126d;
  227.         }      
  228.         if (m1 > 255)
  229.             m1 = 255;
  230.         if (m2 > 255)
  231.             m2 = 255;
  232.         if (m1 < 0)
  233.             m1 = 0;
  234.         if (m2 < 0)
  235.             m2 = 0;
  236.         servo1+=povX*povS;
  237.         servo2+=povY*povS;
  238.         if (servo1 < 0)
  239.             servo1 = 0;
  240.         if (servo1 > 180)
  241.             servo1 = 180;
  242.         if (servo2 < 0)
  243.             servo2 = 0;
  244.         if (servo2 > 180)
  245.             servo2 = 180;    
  246.         if ((joystick.getButtons() & 2) == 2){
  247.             try {
  248.                 net.close();
  249.                 net = new Socket("192.168.100.87",1337);
  250.                 ps = new PrintStream(net.getOutputStream());        
  251.             } catch (UnknownHostException e) {
  252.                 // TODO Auto-generated catch block
  253.                 e.printStackTrace();
  254.             } catch (IOException e) {
  255.                 // TODO Auto-generated catch block
  256.                 e.printStackTrace();
  257.             }  
  258.        
  259.         }
  260.            
  261.         povLabel.setText(((int)m1)+" "+((int)m2)+" "+((int)servo1)+" "+((int)servo2));
  262.         ps.print("c"+((int)m2)+" "+((int)m1)+" "+((int)servo1)+" "+((int)servo2));
  263.         try {
  264.             net.getOutputStream().write((byte)' ');
  265.         } catch(Exception e){
  266.             try {
  267.                 net = new Socket("192.168.100.87",1337);
  268.                 ps = new PrintStream(net.getOutputStream());    
  269.                 System.err.println("REconnect");
  270.                
  271.             } catch (UnknownHostException e1) {
  272.                 // TODO Auto-generated catch block
  273.                 e.printStackTrace();
  274.             } catch (IOException e1) {
  275.                 // TODO Auto-generated catch block
  276.                 e.printStackTrace();
  277.             }
  278.            
  279.         }
  280.         if (((!net.isConnected()) || net.isClosed())){
  281.             try {
  282.                 net = new Socket("192.168.100.87",1337);
  283.                 ps = new PrintStream(net.getOutputStream());        
  284.             } catch (UnknownHostException e) {
  285.                 // TODO Auto-generated catch block
  286.                 e.printStackTrace();
  287.             } catch (IOException e) {
  288.                 // TODO Auto-generated catch block
  289.                 e.printStackTrace();
  290.             }  
  291.         }
  292.     }
  293.  
  294.     public void startPolling() {
  295.         thread.start();
  296.     }
  297.  
  298.     public void addListeners() {
  299.         add(addButton);
  300.         add(removeButton);
  301.         joy.addJoystickListener(this);
  302.         joy2.addJoystickListener(this);
  303.     }
  304.  
  305.     public void setDeadZone(double deadZone) {
  306.         joy.setDeadZone(deadZone);
  307.         updateDeadZone();
  308.     }
  309.  
  310.     public void setDeadZoneEx(double deadZone) {
  311.         joy2.setDeadZone(deadZone);
  312.         updateDeadZone();
  313.     }
  314.  
  315.     public void updateDeadZone() {
  316.         deadZoneLabel.setText("joy(" + joy.getDeadZone() + "), joy#2("
  317.                               + joy2.getDeadZone() + ")");
  318.     }
  319.  
  320.     private static void help() {
  321.         System.out.println("Help:");
  322.         System.out.println(" -h This help screen info");
  323.         System.out.println(" -v Verbose Joystick debug information");
  324.         System.out.println(" -j:n Set the Joystick ID to test (n is an integer)");
  325.         System.out.println(" -j2:n Set the second joystick ID to test (n is an integer)");
  326.         System.out.println(" -d:n Set the dead zone size of the Joystick (n is a real number)");
  327.         System.out.println(" -d2:n Set the dead zone size of the second Joystick (n is a real number)");
  328.     }
  329.  
  330.     public static void main(String args[]) {
  331.         // This first and last one are never there, but this is for internal testing.
  332.         // They should ALWAYS be false.
  333.         try {
  334.             WindowTest mainFrame;
  335.             //WindowTest listenerFrame;
  336.             int joystickNum = -1, joystickNumEx = -1;
  337.             double deadZone = -1.0, deadZoneEx = -1.0;
  338.             int interval = 50;
  339.  
  340.             for (int idx = 0; idx < args.length; idx++) {
  341.                 if (args[idx].startsWith("-d2:")) {
  342.                     deadZoneEx =
  343.                         Double.valueOf(args[idx].substring(4, args[idx].length()))
  344.                         .doubleValue();
  345.                 }
  346.                 else if (args[idx].startsWith("-d:")) {
  347.                     deadZone =
  348.                         Double.valueOf(args[idx].substring(3, args[idx].length()))
  349.                         .doubleValue();
  350.                 }
  351.                 else if (args[idx].startsWith("-i:")) {
  352.                     interval =
  353.                         Integer.valueOf(args[idx].substring(3, args[idx].length()))
  354.                         .intValue();
  355.                 }
  356.                 else if (args[idx].startsWith("-j:")) {
  357.                     joystickNum =
  358.                         Integer.valueOf(args[idx].substring(3, args[idx].length()))
  359.                         .intValue();
  360.                 }
  361.                 else if (args[idx].startsWith("-j2:")) {
  362.                     joystickNumEx =
  363.                         Integer.valueOf(args[idx].substring(4, args[idx].length()))
  364.                         .intValue();
  365.                 }
  366.                 else if (args[idx].startsWith("-v")) {
  367.                     for (int id = -1; id <= Joystick.getNumDevices(); id++) {
  368.                         System.out.println("Joystick " + id + ": " + Joystick.isPluggedIn(id));
  369.                     }
  370.                 }
  371.                 else if (args[idx].startsWith("-h")) {
  372.                     help();
  373.                 }
  374.                 else {
  375.                     System.out.println("Unknown option: " + args[idx]);
  376.                     help();
  377.                 }
  378.             }
  379.             if (joystickNum >= 0) {
  380.                 if (joystickNumEx < 0) {
  381.                     joystickNumEx = joystickNum;
  382.                 }
  383.                 mainFrame = new WindowTest(joystickNum, joystickNumEx);
  384.                 //listenerFrame = new WindowTest(joystickNum, joystickNumEx);
  385.             }
  386.             else {
  387.                 mainFrame = new WindowTest();
  388.                // listenerFrame = new WindowTest();
  389.             }
  390.             if (deadZone >= 0.0) {
  391.                 mainFrame.setDeadZone(deadZone);
  392.                // listenerFrame.setDeadZone(deadZone);
  393.             }
  394.             if (deadZoneEx >= 0.0) {
  395.                 mainFrame.setDeadZoneEx(deadZoneEx);
  396.                // listenerFrame.setDeadZoneEx(deadZoneEx);
  397.             }
  398.             mainFrame.setPollInterval(interval);
  399.             mainFrame.updateDeadZone();
  400.             mainFrame.pack();
  401.             mainFrame.setTitle("Polling Joystick");
  402.             //mainFrame.show();
  403.             mainFrame.setVisible(true);
  404.             mainFrame.startPolling();
  405.  
  406.             //listenerFrame.setPollInterval(interval);
  407.             //listenerFrame.updateDeadZone();
  408.            // listenerFrame.addListeners();
  409.            // listenerFrame.pack();
  410.           // listenerFrame.setTitle("Listener Joystick");
  411.             Point pt = mainFrame.getLocation();
  412.            // pt.x += listenerFrame.getWidth();
  413.            // listenerFrame.setLocation(pt);
  414.             //listenerFrame.show();
  415.            // listenerFrame.setVisible(true);
  416.         } catch (IOException e) {
  417.             System.err.println("");
  418.             System.err.println(e.getMessage());
  419.             System.exit(1);
  420.         }
  421.     }
  422. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!