123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188 |
- 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 javax.swing.JFrame;
- import javax.swing.JOptionPane;
- import javax.swing.JPanel;
- import javax.swing.JSplitPane;
- import javax.swing.SwingUtilities;
- import config.Map;
- import config.SonarModel;
- import lejos.robotics.mapping.LineMap;
- import lejos.robotics.navigation.Pose;
- import robots.DataPose;
- 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 SonarModel smodel;
- 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(map);
- smodel = SonarModel(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);
-
- boolean result = false;
- if (robot != null)
- result = robot.connect(this);
-
- if (result == false) {
- JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
- }
- //
- // String text = "1,2,3 - Change view mode.\n";
- // text += "s - Save image.\n";
- // text += "l - Show trace.\n";
- // text += "c - Clean map.\n";
- // JOptionPane.showMessageDialog(null, text);
- }
- public void addPoint(Pose p) {
- imap.addPoint(p);
- }
- @Override
- public void keyPressed(KeyEvent e) {
- char input = e.getKeyChar();
- if (input == '1')
- imap.setVisual(0);
- else if (input == '2')
- imap.setVisual(1);
- else if (input == '3')
- imap.setVisual(2);
- else if (input == 'l')
- imap.showLine();
- else if (input == 's')
- imap.save();
- else if (input == 'c')
- imap.clean();
- if (robot == null)
- return;
- switch (e.getKeyCode()) {
- case KeyEvent.VK_UP:
- robot.moveForward();
- break;
- case KeyEvent.VK_DOWN:
- robot.moveBackward();
- break;
- case KeyEvent.VK_LEFT:
- robot.moveLeft();
- break;
- case KeyEvent.VK_RIGHT:
- robot.moveRight();
- break;
- }
- }
- @Override
- public void keyReleased(KeyEvent arg0) {
- if (robot == null)
- return;
- robot.stop();
- }
- @Override
- public void keyTyped(KeyEvent arg0) {
- }
- public static void main(String[] args) {
- LineMap map = Map.makeMap();
- Robot robot = new VirtualRobot(map);
- //Robot robot = new BluetoothRobot("NXJ8");
- SwingUtilities.invokeLater(new Runnable() {
- public void run() {
- new MainProgram(map, robot);
- }
- });
- }
- @Override
- public void windowOpened(WindowEvent e) {
- }
- @Override
- public void windowClosing(WindowEvent e) {
- System.err.println("Fechando...");
- if (robot == null)
- return;
- robot.exit();
- }
- @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(DataPose data) {
- // posicao do robo
- Pose p = data.getPose();
- imap.addPoint(p);
-
- // ponto do ultrasonico
- double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
- double dx = Math.cos(sensor_ang)*data.getDistance();
- double dy = Math.sin(sensor_ang)*data.getDistance();
- imap.addRead(p.getX()+dx, p.getY()+dy);
- double expected = SonarModel.expectedRead();
- scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
- }
- }
|