123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381 |
- import java.awt.BorderLayout;
- import java.awt.event.KeyEvent;
- import java.awt.event.KeyListener;
- import java.awt.event.WindowEvent;
- import java.awt.event.WindowListener;
- import java.util.ArrayList;
- import java.util.Iterator;
- import javax.swing.JFrame;
- import javax.swing.JOptionPane;
- import javax.swing.JPanel;
- import javax.swing.JSplitPane;
- import javax.swing.SwingUtilities;
- import config.Map;
- import config.Models;
- import lejos.robotics.mapping.LineMap;
- import lejos.robotics.navigation.Pose;
- import robots.BluetoothRobot;
- import robots.DataRead;
- import robots.LCPRobot;
- import robots.Robot;
- import robots.RobotReturn;
- import robots.VirtualRobot;
- public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
- private MapImage imap;
- private Robot robot;
- private ScannerImage scanner;
- private Models model;
- private ProbabilityMatriz matriz;
- public static final byte FORWARD = 0;
- public static final byte STOP = 1;
- public static final byte EXIT = 2;
- public static final byte LEFT = 3;
- public static final byte RIGHT = 4;
- public static final byte BACKWARD = 5;
- public MainProgram(LineMap map, Robot robot) {
- this.robot = robot;
- JFrame frame = new JFrame("Mapa MAC0318");
- frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
- frame.setLayout(new BorderLayout());
- imap = new MapImage(map);
- scanner = new ScannerImage();
- model = new Models(map);
- // frame.add(this.map);
- frame.setSize(800, 800);
- frame.setVisible(true);
- frame.setFocusable(true);
- frame.requestFocusInWindow();
- frame.addKeyListener(this);
- frame.addWindowListener(this);
- JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
- splitPane.setOneTouchExpandable(true);
- splitPane.setDividerLocation((int) (frame.getHeight() / 2));
- frame.add(splitPane);
- matriz = new ProbabilityMatriz(map.getBoundingRect(), 10, 9);
-
- crenca();
- // showHelp();
- }
- private void crenca() {
- for (DiscretePoint p: matriz) {
- // System.out.println(n(p.midAng()));
- if (p.z == 4 && p.x == 3 && p.y == 3)
- p.set(1);
- else
- p.set(0);
- }
- matriz.normalize();
- imap.showMatriz(matriz);
- }
-
- private double n (double x) {
- x = Math.toRadians(x);
- return Math.toDegrees(Math.atan2(Math.sin(x), Math.cos(x)));
- }
- private void showHelp() {
- String text = "1,2,3 - Change global map view\n";
- text += "l - Show global map trace.\n";
- text += "c - Clean maps.\n";
- text += "m - Enter robot movement.\n";
- text += "r - Enter robo rotation.\n";
- text += "a - Colect sonar data.\n";
- text += "z - Make sonar continuous scanner.\n";
- text += "i - Stop continuous scanner.\n";
- text += "g - Save global map image.\n";
- text += "f - Save scanner image.\n";
- text += "<arrows> - Move robot.\n";
- text += "h - help.\n";
- JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
- }
- public void addPoint(Pose p) {
- imap.addPoint(p);
- }
- @Override
- public void keyPressed(KeyEvent e) {
- char input = e.getKeyChar();
- switch (input) {
- case '1':
- imap.setVisual(0);
- break;
- case '2':
- imap.setVisual(1);
- break;
- case '3':
- imap.setVisual(2);
- break;
- case 'l':
- imap.showLine();
- break;
- case 'g':
- imap.save();
- break;
- case 'f':
- scanner.save();
- break;
- case 'c':
- imap.clean();
- scanner.clean();
- break;
- case 'm':
- moveRobot();
- break;
- case 'r':
- rotateRobot();
- break;
- case 'b':
- crenca();
- break;
- case 'a':
- colectSonar();
- break;
- case 'h':
- showHelp();
- break;
- case 's':
- // reseta a contagem
- System.out.println("resetou");
- matriz.setAll(1);
- matriz.normalize();
- imap.repaint();
- break;
- default:
- break;
- }
- if (robot == null)
- return;
- switch (e.getKeyCode()) {
- case KeyEvent.VK_UP:
- robot.moveForward();
- scanner.clean();
- break;
- case KeyEvent.VK_DOWN:
- robot.moveBackward();
- scanner.clean();
- break;
- case KeyEvent.VK_LEFT:
- robot.moveLeft();
- scanner.clean();
- break;
- case KeyEvent.VK_RIGHT:
- robot.moveRight();
- scanner.clean();
- break;
- }
- }
- private void colectSonar() {
- int interval;
- try {
- String rs = JOptionPane.showInputDialog("Interval (degress):");
- interval = Integer.parseInt(rs);
- } catch (Exception e) {
- return;
- }
- ArrayList<DataRead> data = robot.scann(-90, 90, interval);
- if (data == null) return;
- /* PROBABILIDADES DEPOIS LEITURA */
- for (DiscretePoint p: matriz) {
- Pose pose = p.pose();
- double prob = model.readProbability(pose, data);
- p.set(prob*p.get());
- }
- matriz.normalize();
- imap.setMatriz(matriz);
-
- for (DataRead d : data) {
- robotData(d);
- }
- System.out.println("... "+matriz.sum());
- imap.repaint();
- }
- private void rotateRobot() {
- try {
- String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
- double r = Double.parseDouble(rs);
- robot.rotate(r);
-
- rotateAndMoveRobot(0, r);
- } catch (Exception e) {
- }
- }
- private void moveRobot() {
- try {
- String rs = JOptionPane.showInputDialog("Enter distance (cm):");
- double r = Double.parseDouble(rs);
- robot.move(r);
-
- rotateAndMoveRobot(r, 0);
- } catch (Exception e) {
- }
- }
-
- private void rotateAndMoveRobot(double move, double ang) {
- /* PROBABILIDADES DEPOIS MOVIMENTO */
- ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
- System.out.print("Iniciando...");
-
- // Verifica o numero de cores
- int cores = Runtime.getRuntime().availableProcessors();
-
- // tudo isso
- ArrayList<Thread> threads = new ArrayList<Thread>();
- for (int i = 0; i < cores; i++) {
- Iterator<DiscretePoint> iteretor = newmatriz.iteratorSet((newmatriz.totalsize*i)/cores, newmatriz.totalsize*(i+1)/cores);
- Thread thread = new Thread () {
- public void run() {
- while(iteretor.hasNext()) {
- DiscretePoint newp = iteretor.next();
- double sum = 0;
- for (DiscretePoint p: matriz) {
- Pose pa = p.pose();
- Pose pb = newp.pose();
- sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
- }
- newp.set(sum);
- }
- }
- };
- thread.start();
- threads.add(thread);
- }
-
- for (Thread thread: threads) {
- try {
- thread.join();
- } catch (InterruptedException e) {
- }
- }
-
- // OU ISSO
- // for (DiscretePoint newp: newmatriz) {
- // double sum = 0;
- // for (DiscretePoint p: matriz) {
- // Pose pa = p.pose();
- // Pose pb = newp.pose();
- // sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
- // }
- // newp.set(sum);
- // }
-
- matriz = newmatriz;
- imap.setMatriz(matriz);
- matriz.normalize();
- System.out.println("Finalizado "+matriz.sum());
- scanner.clean();
- imap.repaint();
- }
- @Override
- public void keyReleased(KeyEvent arg0) {
- if (robot == null)
- return;
- robot.stop();
- }
- @Override
- public void keyTyped(KeyEvent arg0) {
- }
- @Override
- public void windowOpened(WindowEvent e) {
- }
- @Override
- public void windowClosing(WindowEvent e) {
- System.err.println("Fechando...");
- if (robot == null)
- return;
- robot.disconnect();
- }
- @Override
- public void windowClosed(WindowEvent e) {
- }
- @Override
- public void windowIconified(WindowEvent e) {
- }
- @Override
- public void windowDeiconified(WindowEvent e) {
- }
- @Override
- public void windowActivated(WindowEvent e) {
- }
- @Override
- public void windowDeactivated(WindowEvent e) {
- }
- @Override
- public void robotData(DataRead data) {
- // posicao do robo
- // System.out.print(" sang:"+data.getSensorAngle());
- // System.out.println(" distance:"+data.getDistance());
- scanner.addRead(data.getDistance(), data.getSensorAngle()-90);
- }
- public static void main(String[] args) {
- LineMap map = Map.makeMap();
- Robot robotv = new VirtualRobot(map);
- Robot robotbt = new BluetoothRobot("NXT02");
- String s = "LCPRobot";
- Object[] possibleValues = { robotv, robotbt, s };
- Object robottmp = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
- possibleValues, possibleValues[0]);
- Robot robot;
-
-
- boolean result = false;
- if (robottmp == s) {
- robot = new LCPRobot();
- } else {
- robot = (Robot) robottmp;
- }
- if (robot != null)
- result = ((Robot) robot).connect();
- if (result == false) {
- JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
- System.exit(ERROR);
- }
- SwingUtilities.invokeLater(new Runnable() {
- public void run() {
- new MainProgram(map, (Robot) robot);
- }
- });
- }
- }
|