diff --git a/src/com/muhavision/Main.java b/src/com/muhavision/Main.java index 831a979..432647a 100644 --- a/src/com/muhavision/Main.java +++ b/src/com/muhavision/Main.java @@ -13,26 +13,19 @@ //Imports (obvious) import java.awt.Color; import java.awt.Font; +import java.awt.Frame; import java.awt.GridLayout; import java.awt.event.ActionEvent; import java.awt.event.ActionListener; -import java.awt.event.KeyEvent; -import java.awt.event.KeyListener; -import java.awt.event.MouseEvent; -import java.awt.event.MouseMotionListener; -import java.io.BufferedReader; import java.io.IOException; -import java.io.InputStreamReader; import java.net.DatagramPacket; import java.net.DatagramSocket; import java.net.SocketException; -import java.util.Scanner; import javax.swing.JButton; import javax.swing.JFrame; import javax.swing.JPanel; -import com.codeminders.ardrone.ARDrone.Animation; import com.muhavision.control.DroneController; import com.muhavision.control.ExpoController; import com.muhavision.control.FlightMode; @@ -40,28 +33,34 @@ //This is a class public class Main { - //Graphics (don't know what it does) + // Graphics (don't know what it does) JFrame controlTowerFrame = new JFrame("Muha Mission Planner"); - + private final static int PACKETSIZE = 1024; - + + // Let's start this and mami!!! + public static void main(String[] args) { + new Main(); + } + public float roll, pitch, yaw, height; - VisualRenderer visual = new VisualRenderer(this); + DroneController controller = new DroneController(visual); - + public static final boolean DEBUG = true; - public FlightMode flightMode = new FlightMode(); + public FlightMode flightMode = new FlightMode(); public Main() { - //More graphics - if(!DEBUG) new SplashScreen(); - + // More graphics + if (!DEBUG) + new SplashScreen(); + JPanel commands = new JPanel(); - + commands.setBackground(Color.black); - + JButton takeoff = new JButton("Take off"); takeoff.setBackground(Color.black); takeoff.setForeground(Color.white); @@ -78,237 +77,241 @@ public void actionPerformed(ActionEvent e) { }); takeoff.setFocusable(false); commands.add(takeoff); - - //main panel + + // main panel JPanel visualHolder = new JPanel(new GridLayout()); visualHolder.add(visual); controlTowerFrame.add("Center", visualHolder); - + JButton land = new JButton("Land"); land.setBackground(Color.black); land.setForeground(Color.white); land.setFont(new Font("Arial", Font.PLAIN, 17)); land.addActionListener(new ActionListener() { - @Override - public void actionPerformed(ActionEvent e) { - try { - controller.getDrone().land(); - } catch (IOException e1) { - e1.printStackTrace(); - } - } - }); + @Override + public void actionPerformed(ActionEvent e) { + try { + controller.getDrone().land(); + } catch (IOException e1) { + e1.printStackTrace(); + } + } + }); land.setFocusable(false); commands.add(land); - + JButton trim = new JButton("Flat trim"); trim.setBackground(Color.black); trim.setForeground(Color.white); trim.setFont(new Font("Arial", Font.PLAIN, 17)); trim.addActionListener(new ActionListener() { - @Override - public void actionPerformed(ActionEvent e) { - try { - controller.getDrone().trim(); - } catch (IOException e1) { - e1.printStackTrace(); - } - } - }); + @Override + public void actionPerformed(ActionEvent e) { + try { + controller.getDrone().trim(); + } catch (IOException e1) { + e1.printStackTrace(); + } + } + }); trim.setFocusable(false); commands.add(trim); - + controlTowerFrame.add("North", commands); - - //controlTowerFrame.setResizable(false); - //controlTowerFrame.setSize(700, 500); - - //controlTowerFrame.setUndecorated(true); - //Now graphics may be visible. + // controlTowerFrame.setResizable(false); + // controlTowerFrame.setSize(700, 500); + + // controlTowerFrame.setUndecorated(true); + + // Now graphics may be visible. controlTowerFrame.setVisible(true); - controlTowerFrame.setExtendedState(JFrame.MAXIMIZED_BOTH); - //controlTowerFrame.setFocusable(true); - //controlTowerFrame.setFocusableWindowState(true); + controlTowerFrame.setExtendedState(Frame.MAXIMIZED_BOTH); + // controlTowerFrame.setFocusable(true); + // controlTowerFrame.setFocusableWindowState(true); controlTowerFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - /*controlTowerFrame.addKeyListener(new KeyListener() { - @Override public void keyTyped(KeyEvent arg0) {} - @Override public void keyReleased(KeyEvent arg0) { - - //Let's listen for some keys!!! - if(arg0.getKeyChar()=='w') pitch = 0; - if(arg0.getKeyChar()=='s') pitch = 0; - if(arg0.getKeyChar()=='a') roll = 0; - if(arg0.getKeyChar()=='d') roll = 0; - - reloadControls(); - } + /* + * controlTowerFrame.addKeyListener(new KeyListener() { + * + * @Override public void keyTyped(KeyEvent arg0) {} + * + * @Override public void keyReleased(KeyEvent arg0) { + * + * //Let's listen for some keys!!! if(arg0.getKeyChar()=='w') pitch = 0; + * if(arg0.getKeyChar()=='s') pitch = 0; if(arg0.getKeyChar()=='a') roll + * = 0; if(arg0.getKeyChar()=='d') roll = 0; + * + * reloadControls(); } + * + * //Let's listen for some more keys!!! + * + * @Override public void keyPressed(KeyEvent arg0) { + * if(arg0.getKeyChar()=='\n') try { + * controller.getDrone().sendEmergencySignal(); } catch (IOException e) + * { e.printStackTrace(); } if(arg0.getKeyCode()==KeyEvent.VK_ESCAPE){ + * controlTowerFrame.setVisible(false); try { + * controller.getDrone().sendEmergencySignal(); } catch (IOException e) + * { e.printStackTrace(); } } + * + * if(arg0.getKeyChar()=='w') pitch = -10; if(arg0.getKeyChar()=='s') + * pitch = 10; if(arg0.getKeyChar()=='a') roll = -10; + * if(arg0.getKeyChar()=='d') roll = 10; + * + * reloadControls(); + * + * } }); + */ - //Let's listen for some more keys!!! - @Override - public void keyPressed(KeyEvent arg0) { - if(arg0.getKeyChar()=='\n') - try { - controller.getDrone().sendEmergencySignal(); - } catch (IOException e) { - e.printStackTrace(); - } - if(arg0.getKeyCode()==KeyEvent.VK_ESCAPE){ - controlTowerFrame.setVisible(false); - try { - controller.getDrone().sendEmergencySignal(); - } catch (IOException e) { - e.printStackTrace(); + // Now lets listen for mami!!! + try { + + final DatagramSocket socket = new DatagramSocket(1234); + + Thread mami_listener = new Thread() { + + @Override + public void run() { + + // On port 1234 mami sends x,y,z,a axis. + while (true) { + // x;y;z + DatagramPacket packet = new DatagramPacket( + new byte[PACKETSIZE], PACKETSIZE); + + try { + socket.receive(packet); + } catch (IOException e) { + e.printStackTrace(); + } + + String[] mami_array = new String(packet.getData()) + .split("\\;"); + + if (mami_array.length > 1) { + + pitch = (float) (ExpoController.getExpo(Integer + .parseInt(mami_array[1].trim()))); + roll = (float) (ExpoController.getExpo(Integer + .parseInt(mami_array[0].trim()))); + yaw = (float) (ExpoController.getExpo((Integer + .parseInt(mami_array[2].trim()))) / 4.7);// reduce + // response + int mamih = Integer.parseInt(mami_array[3].trim()); + if (mamih == 2) + height = -3; + else if (mamih == -1) + height = 3; + else + height = 0; + + reloadControls(); + + } } } - - if(arg0.getKeyChar()=='w') pitch = -10; - if(arg0.getKeyChar()=='s') pitch = 10; - if(arg0.getKeyChar()=='a') roll = -10; - if(arg0.getKeyChar()=='d') roll = 10; - - reloadControls(); - - } - });*/ - //Now lets listen for mami!!! + }; + + mami_listener.start(); + + } catch (SocketException e1) { + e1.printStackTrace(); + } + + // Lets listen for some more mami!!! try { - - final DatagramSocket socket = new DatagramSocket(1234); - - Thread mami_listener = new Thread(){ - - public void run(){ - - //On port 1234 mami sends x,y,z,a axis. - while(true){ - // x;y;z - DatagramPacket packet = new DatagramPacket( new byte[PACKETSIZE], PACKETSIZE ) ; - - try { - socket.receive(packet) ; - } catch (IOException e) { - e.printStackTrace(); - } - - String[] mami_array = new String(packet.getData()).split("\\;"); - - if(mami_array.length>1){ - - pitch = (float)(ExpoController.getExpo(Integer.parseInt(mami_array[1].trim()))); - roll = (float)(ExpoController.getExpo(Integer.parseInt(mami_array[0].trim()))); - yaw = (float)(ExpoController.getExpo((Integer.parseInt(mami_array[2].trim())))/4.7);//reduce response - int mamih = Integer.parseInt(mami_array[3].trim()); - if(mamih==2) height = -3; - else if(mamih == -1) height = 3; - else height = 0; - - reloadControls(); - + + final DatagramSocket socket = new DatagramSocket(2345); + + // On port 2345 mami sends button presses. + Thread mami_listener = new Thread() { + + @Override + public void run() { + while (true) { + // x;y;z + DatagramPacket packet = new DatagramPacket( + new byte[PACKETSIZE], PACKETSIZE); + try { + socket.receive(packet); + } catch (IOException e) { + e.printStackTrace(); + } + + String[] mami_array = new String(packet.getData()) + .split("\\;"); + + if (mami_array.length > 1) { + + if (Integer.parseInt(mami_array[0].trim()) == 1) { + try { + controller.getDrone().sendEmergencySignal(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + if (Integer.parseInt(mami_array[3].trim()) == 1) { + try { + controller.getDrone().land(); + } catch (IOException e1) { + e1.printStackTrace(); + } + } + + else if (Integer.parseInt(mami_array[2].trim()) == 1) { + try { + controller.getDrone().takeOff(); + } catch (IOException e1) { + e1.printStackTrace(); + } + } + + else if (Integer.parseInt(mami_array[10].trim()) == 1) { + flightMode + .setMode(FlightMode.eMode.NORMAL_MODE); + } + + else if (Integer.parseInt(mami_array[8].trim()) == 1) { + flightMode.setMode(FlightMode.eMode.MUHA_MODE); + } + + else if (Integer.parseInt(mami_array[6].trim()) == 1) { + flightMode.setMode(FlightMode.eMode.TAG_MODE); + } + + else if (Integer.parseInt(mami_array[1].trim()) == 1) { + try { + controller.getDrone().trim(); + } catch (IOException e1) { + e1.printStackTrace(); + } + } + + else if (Integer.parseInt(mami_array[4].trim()) == 1) { + controller.getDrone().setCombinedYawMode(true); + } + + else if (Integer.parseInt(mami_array[5].trim()) == 1) { + controller.getDrone().setCombinedYawMode(false); + } + + } } } - } - - }; - - mami_listener.start(); - + + }; + + mami_listener.start(); + } catch (SocketException e1) { e1.printStackTrace(); } - - //Lets listen for some more mami!!! - try { - - final DatagramSocket socket = new DatagramSocket(2345); - - //On port 2345 mami sends button presses. - Thread mami_listener = new Thread(){ - - public void run(){ - while(true){ - // x;y;z - DatagramPacket packet = new DatagramPacket( new byte[PACKETSIZE], PACKETSIZE ) ; - try { - socket.receive(packet) ; - } catch (IOException e) { - e.printStackTrace(); - } - - String[] mami_array = new String(packet.getData()).split("\\;"); - - if(mami_array.length>1){ - - if(Integer.parseInt(mami_array[0].trim()) == 1){ - try { - controller.getDrone().sendEmergencySignal(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - if(Integer.parseInt(mami_array[2].trim()) == 1){ - try { - controller.getDrone().takeOff(); - } catch (IOException e1) { - e1.printStackTrace(); - } - } - - if(Integer.parseInt(mami_array[3].trim()) == 1){ - try { - controller.getDrone().land(); - } catch (IOException e1) { - e1.printStackTrace(); - } - } - - if(Integer.parseInt(mami_array[10].trim()) == 1){ - flightMode.setMode(FlightMode.eMode.NORMAL_MODE); - } - - if(Integer.parseInt(mami_array[8].trim()) == 1){ - flightMode.setMode(FlightMode.eMode.MUHA_MODE); - } - - if(Integer.parseInt(mami_array[6].trim()) == 1){ - flightMode.setMode(FlightMode.eMode.TAG_MODE); - } - - if(Integer.parseInt(mami_array[1].trim()) == 1){ - try { - controller.getDrone().trim(); - } catch (IOException e1) { - e1.printStackTrace(); - } - } - - if(Integer.parseInt(mami_array[4].trim()) == 1){ - controller.getDrone().setCombinedYawMode(true); - } - - if(Integer.parseInt(mami_array[5].trim()) == 1){ - controller.getDrone().setCombinedYawMode(false); - } - - } - } - } - - }; - - mami_listener.start(); - - } catch (SocketException e1) { - e1.printStackTrace(); - } - visual.reloadDatas(null, null, null, null); - visual.setDataProps(controlTowerFrame); + } - + protected void reloadControls() { try { controller.getDrone().move(roll, pitch, height, yaw); @@ -317,10 +320,4 @@ protected void reloadControls() { } } - - //Let's start this and mami!!! - public static void main(String[] args) { - new Main(); - } - } diff --git a/src/com/muhavision/SplashScreen.java b/src/com/muhavision/SplashScreen.java index 9f54e59..fe22b09 100644 --- a/src/com/muhavision/SplashScreen.java +++ b/src/com/muhavision/SplashScreen.java @@ -8,38 +8,45 @@ import javax.swing.JFrame; import javax.swing.JLabel; -public class SplashScreen extends JFrame{ +public class SplashScreen extends JFrame { - //Very nice loading screen + /** + * + */ + private static final long serialVersionUID = 1L; + + public static void main(String[] args) { + new SplashScreen(); + } + + // Very nice loading screen public SplashScreen() { - //Some graphic magic I don't want to know what it does. + // Some graphic magic I don't want to know what it does. setUndecorated(true); setSize(600, 200); Dimension dim = Toolkit.getDefaultToolkit().getScreenSize(); - setLocation(dim.width/2-getSize().width/2, - dim.height/2-getSize().height/2); + setLocation(dim.width / 2 - getSize().width / 2, dim.height / 2 + - getSize().height / 2); add(new JLabel(new ImageIcon("./res/logo.png"))); setVisible(true); for (int i = 0; i < 5; i++) { setLocation( - (dim.width/2-getSize().width/2)+((int)(Math.random()*1000)-500), - (dim.height/2-getSize().height/2)+((int)(Math.random()*1000)-500)); + (dim.width / 2 - getSize().width / 2) + + ((int) (Math.random() * 1000) - 500), + (dim.height / 2 - getSize().height / 2) + + ((int) (Math.random() * 1000) - 500)); try { Thread.sleep(200); } catch (InterruptedException e) { e.printStackTrace(); } } - setLocation(dim.width/2-getSize().width/2, - dim.height/2-getSize().height/2); + setLocation(dim.width / 2 - getSize().width / 2, dim.height / 2 + - getSize().height / 2); add(new JLabel(new ImageIcon("./res/logo.png"))); setVisible(false); } - - public static void main(String[] args) { - new SplashScreen(); - } - + } -//Done with graphics (for now). +// Done with graphics (for now). diff --git a/src/com/muhavision/control/DroneController.java b/src/com/muhavision/control/DroneController.java index 20ae5a2..a24b3c2 100644 --- a/src/com/muhavision/control/DroneController.java +++ b/src/com/muhavision/control/DroneController.java @@ -14,37 +14,38 @@ import com.muhavision.pid.PID; public class DroneController { - + PID roll = new PID(1, 1, 0, 10, PID.Direction.NORMAL); - + OpticalFlowCalculator calc = new OpticalFlowCalculator(); - + ARDrone drone = null; NavData data = null; - + public DroneController(final VisualRenderer visual) { System.out.println("Drone controller loading..."); try { - + drone = new ARDrone(); drone.connect(); drone.addImageListener(new BufferedImageVideoListener() { - + @Override public void imageReceived(BufferedImage image) { QuadrantFlowSpeed speed = null; EulerAngles angle = null; - - if(visual.global_main.flightMode.getMode()==FlightMode.eMode.MUHA_MODE) + + if (visual.global_main.flightMode.getMode() == FlightMode.eMode.MUHA_MODE) speed = calc.getFlowData(image); - else if(visual.global_main.flightMode.getMode()==FlightMode.eMode.TAG_MODE) - angle = MarkerControl.getControlDataAndPictureDataBasedOnNavData(data); - + else if (visual.global_main.flightMode.getMode() == FlightMode.eMode.TAG_MODE) + angle = MarkerControl + .getControlDataAndPictureDataBasedOnNavData(data); + visual.reloadDatas(image, speed, data, angle); } }); - + drone.addNavDataListener(new NavDataListener() { @Override @@ -52,15 +53,15 @@ public void navDataReceived(NavData fdata) { data = fdata; } }); - + } catch (UnknownHostException e) { e.printStackTrace(); } catch (IOException e) { e.printStackTrace(); } } - - public ARDrone getDrone(){ + + public ARDrone getDrone() { return drone; } diff --git a/src/com/muhavision/control/EulerAngles.java b/src/com/muhavision/control/EulerAngles.java index d50a651..a56c2d5 100644 --- a/src/com/muhavision/control/EulerAngles.java +++ b/src/com/muhavision/control/EulerAngles.java @@ -3,11 +3,11 @@ public class EulerAngles { public float roll, pitch, yaw; - + public int picX, picY; - + public int dist; - + public boolean hasAngles = false; - + } diff --git a/src/com/muhavision/control/ExpoController.java b/src/com/muhavision/control/ExpoController.java index 5d13ca5..0bcf6b1 100644 --- a/src/com/muhavision/control/ExpoController.java +++ b/src/com/muhavision/control/ExpoController.java @@ -5,10 +5,11 @@ */ public class ExpoController { - //If it crashes it's because of this + mami ne dotikaj se. - public static double getExpo(int power){ - if (power<0) return-(0.6 * Math.exp(-0.035*(double)power) - 0.6); - return 0.6 * Math.exp(0.035*(double)power) - 0.6; - } - + // If it crashes it's because of this + mami ne dotikaj se. + public static double getExpo(int power) { + if (power < 0) + return -(0.6 * Math.exp(-0.035 * power) - 0.6); + return 0.6 * Math.exp(0.035 * power) - 0.6; + } + } diff --git a/src/com/muhavision/control/FlightMode.java b/src/com/muhavision/control/FlightMode.java index a28c469..2627a99 100644 --- a/src/com/muhavision/control/FlightMode.java +++ b/src/com/muhavision/control/FlightMode.java @@ -5,42 +5,37 @@ */ public class FlightMode { - public enum eMode { + public enum eMode { - NORMAL_MODE(0), - MUHA_MODE(1), - TAG_MODE(2); + NORMAL_MODE(0), MUHA_MODE(1), TAG_MODE(2); - int mode = 0; + int mode = 0; - eMode(int mode){ - this.mode = mode; - } + eMode(int mode) { + this.mode = mode; + } - } - - public static String getFlightModeName(eMode mode){ - if(mode == eMode.NORMAL_MODE) - return "NORMAL"; - if(mode == eMode.MUHA_MODE) - return "MUHA"; - if(mode == eMode.TAG_MODE) - return "TAG"; - return "UNKNOWN"; - } + } - private eMode mode = eMode.NORMAL_MODE; - - public eMode getMode(){ - return mode; - } - - public void setMode(eMode mode){ - this.mode = mode; - mode.toString(); - } + public static String getFlightModeName(eMode mode) { + if (mode == eMode.NORMAL_MODE) + return "NORMAL"; + if (mode == eMode.MUHA_MODE) + return "MUHA"; + if (mode == eMode.TAG_MODE) + return "TAG"; + return "UNKNOWN"; + } + private eMode mode = eMode.NORMAL_MODE; + public eMode getMode() { + return mode; + } + public void setMode(eMode mode) { + this.mode = mode; + mode.toString(); + } } diff --git a/src/com/muhavision/control/JoystickController.java b/src/com/muhavision/control/JoystickController.java index 318d4ca..8a2251b 100644 --- a/src/com/muhavision/control/JoystickController.java +++ b/src/com/muhavision/control/JoystickController.java @@ -5,8 +5,8 @@ */ public class JoystickController { - public static void readUSB() { - - } + public static void readUSB() { + + } } diff --git a/src/com/muhavision/control/MarkerControl.java b/src/com/muhavision/control/MarkerControl.java index c773793..78b669e 100644 --- a/src/com/muhavision/control/MarkerControl.java +++ b/src/com/muhavision/control/MarkerControl.java @@ -1,6 +1,5 @@ package com.muhavision.control; -import java.util.ArrayList; import java.util.List; import com.codeminders.ardrone.NavData; @@ -8,14 +7,15 @@ import com.codeminders.ardrone.VisionTag; public class MarkerControl { - - public static EulerAngles getControlDataAndPictureDataBasedOnNavData(NavData nav){ - - if(nav.isVisionEnabled()){ - List tags = (ArrayList) nav.getVisionTags(); - if(!tags.isEmpty()){ - VisionTag tag = tags.get(0); - if(tag!=null){ + + public static EulerAngles getControlDataAndPictureDataBasedOnNavData( + NavData nav) { + + if (nav.isVisionEnabled()) { + List tags = nav.getVisionTags(); + if (!tags.isEmpty()) { + VisionTag tag = tags.get(0); + if (tag != null) { EulerAngles angles = new EulerAngles(); Point point = tag.getPosition(); angles.picX = point.getX(); @@ -26,7 +26,7 @@ public static EulerAngles getControlDataAndPictureDataBasedOnNavData(NavData nav } } } - + return new EulerAngles(); } diff --git a/src/com/muhavision/cv/OpticalFlowCalculator.java b/src/com/muhavision/cv/OpticalFlowCalculator.java index 47b8882..0a69535 100644 --- a/src/com/muhavision/cv/OpticalFlowCalculator.java +++ b/src/com/muhavision/cv/OpticalFlowCalculator.java @@ -1,204 +1,222 @@ package com.muhavision.cv; +import static org.bytedeco.javacpp.opencv_core.CV_TERMCRIT_EPS; +import static org.bytedeco.javacpp.opencv_core.CV_TERMCRIT_ITER; +import static org.bytedeco.javacpp.opencv_core.IPL_DEPTH_32F; +import static org.bytedeco.javacpp.opencv_core.IPL_DEPTH_8U; +import static org.bytedeco.javacpp.opencv_core.cvCreateImage; +import static org.bytedeco.javacpp.opencv_core.cvGetSize; +import static org.bytedeco.javacpp.opencv_core.cvSize; +import static org.bytedeco.javacpp.opencv_core.cvTermCriteria; +import static org.bytedeco.javacpp.opencv_imgproc.CV_BGR2GRAY; +import static org.bytedeco.javacpp.opencv_imgproc.cvCvtColor; +import static org.bytedeco.javacpp.opencv_imgproc.cvFindCornerSubPix; +import static org.bytedeco.javacpp.opencv_imgproc.cvGoodFeaturesToTrack; +import static org.bytedeco.javacpp.opencv_video.cvCalcOpticalFlowPyrLK; + import java.awt.image.BufferedImage; import java.util.Vector; import org.bytedeco.javacpp.BytePointer; import org.bytedeco.javacpp.FloatPointer; import org.bytedeco.javacpp.IntPointer; +import org.bytedeco.javacpp.opencv_core.CvPoint2D32f; +import org.bytedeco.javacpp.opencv_core.CvSize; import org.bytedeco.javacpp.opencv_core.IplImage; - -import com.codeminders.ardrone.Point; - -import static org.bytedeco.javacpp.opencv_core.*; -import static org.bytedeco.javacpp.opencv_imgproc.*; -import static org.bytedeco.javacpp.opencv_video.*; -import static org.bytedeco.javacpp.opencv_highgui.*; +import org.bytedeco.javacpp.helper.opencv_core.AbstractIplImage; +import org.bytedeco.javacpp.helper.opencv_core.CvArr; public class OpticalFlowCalculator { - + BufferedImage prev = null; - + private static final int MAX_CORNERS = 190; - - public BufferedImage Equalize(BufferedImage bufferedimg) - { - IplImage iploriginal = IplImage.createFrom(bufferedimg); - IplImage srcimg = IplImage.create(iploriginal.width(), iploriginal.height(), IPL_DEPTH_8U, 1); - //IplImage destimg = IplImage.create(iploriginal.width(), iploriginal.height(), IPL_DEPTH_8U, 1); + public BufferedImage Equalize(BufferedImage bufferedimg) { + IplImage iploriginal = AbstractIplImage.createFrom(bufferedimg); + + IplImage srcimg = AbstractIplImage.create(iploriginal.width(), + iploriginal.height(), IPL_DEPTH_8U, 1); + // IplImage destimg = IplImage.create(iploriginal.width(), + // iploriginal.height(), IPL_DEPTH_8U, 1); - cvCvtColor(iploriginal, srcimg, CV_BGR2GRAY); + cvCvtColor(iploriginal, srcimg, CV_BGR2GRAY); - //cvEqualizeHist( srcimg, destimg ); + // cvEqualizeHist( srcimg, destimg ); - BufferedImage eqimg = srcimg.getBufferedImage(); + BufferedImage eqimg = srcimg.getBufferedImage(); - return eqimg; - } - - public QuadrantFlowSpeed getFlowData(BufferedImage curr){ - - //first time - if(prev==null){ - prev=curr; + return eqimg; + } + + public QuadrantFlowSpeed getFlowData(BufferedImage curr) { + + // first time + if (prev == null) { + prev = curr; return new QuadrantFlowSpeed(); } - - IplImage current_color = IplImage.createFrom(curr); - IplImage previous_color = IplImage.createFrom(prev); - - IplImage current = IplImage.create(current_color.width(), current_color.height(), IPL_DEPTH_8U, 1); - IplImage previous = IplImage.create(previous_color.width(), previous_color.height(), IPL_DEPTH_8U, 1); - + + IplImage current_color = AbstractIplImage.createFrom(curr); + IplImage previous_color = AbstractIplImage.createFrom(prev); + + IplImage current = AbstractIplImage.create(current_color.width(), + current_color.height(), IPL_DEPTH_8U, 1); + IplImage previous = AbstractIplImage.create(previous_color.width(), + previous_color.height(), IPL_DEPTH_8U, 1); + cvCvtColor(current_color, current, CV_BGR2GRAY); cvCvtColor(previous_color, previous, CV_BGR2GRAY); - + CvSize velikost = cvGetSize(current); - + IplImage eig_image = cvCreateImage(velikost, IPL_DEPTH_32F, 1); - IplImage tmp_image = cvCreateImage(velikost, IPL_DEPTH_32F, 1); - - IntPointer corner_count = new IntPointer(1).put(MAX_CORNERS); - CvPoint2D32f cornersA = new CvPoint2D32f(MAX_CORNERS); - - CvArr mask = null; - cvGoodFeaturesToTrack(current, eig_image, tmp_image, cornersA, - corner_count, 0.01, 5.0, mask, 3, 0, 0.04); - - int win_size = 5; - - cvFindCornerSubPix(current, cornersA, corner_count.get(), - cvSize(win_size, win_size), cvSize(-1, -1), - cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); - - //Lucas Kande filter - - BytePointer features_found = new BytePointer(MAX_CORNERS); - FloatPointer feature_errors = new FloatPointer(MAX_CORNERS); - - CvSize pyr_sz = cvSize(current.width() + 8, previous.height() / 3); - - IplImage pyrA = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1); - IplImage pyrB = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1); - - CvPoint2D32f cornersB = new CvPoint2D32f(MAX_CORNERS); - cvCalcOpticalFlowPyrLK(current, previous, pyrA, pyrB, cornersA, cornersB, - corner_count.get(), cvSize(win_size, win_size), 5, - features_found, feature_errors, - cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3), 0); - - Vector left = new Vector(); - Vector right = new Vector(); - - float avgsum_left = 0; - float left_number = 0; - float avgsum_right = 0; - float right_number = 0; - - for (int i = 0; i < corner_count.get(); i++) { - if (features_found.get(i) == 0 || feature_errors.get(i) > 550) { - continue; - } - cornersA.position(i); - cornersB.position(i); - int ax = (int) cornersA.x(); - //int ay = (int) cornersA.y(); - int bx = (int) cornersB.x(); - //int by = (int) cornersB.y(); - if(ax<100){ - left.add(new VectorData(ax, 0, bx, 0));//left.add(new VectorData(ax, ay, bx, by)); - int dx = Math.abs(ax-bx); - //int dy = Math.abs(ay-by); - //float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); - avgsum_left += dx; - left_number++; - }else if(ax>220){ - right.add(new VectorData(ax, 0, bx, 0));//right.add(new VectorData(ax, ay, bx, by)); - int dx = Math.abs(ax-bx); - //int dy = Math.abs(ay-by); - //float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); - avgsum_right += dx; - right_number++; - } - } - - float avgl = avgsum_left/left_number; - float avgr = avgsum_right/right_number; - - int avglx1_sum = 0; - //int avgly1_sum = 0; - int avglx2_sum = 0; - //int avgly2_sum = 0; - - int vecltime = 0; - - if(left_number>0) - for(VectorData point : left){ - int dx = Math.abs(point.x1 - point.x2); - //int dy = Math.abs(point.y1 - point.y2); - //float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); - if((dx/avgl)<5){ - avglx1_sum += point.x1; - avglx2_sum += point.x2; - //avgly1_sum += point.y1; - //avgly2_sum += point.y2; - vecltime++; - } - } - - int avgrx1_sum = 0; - //int avgry1_sum = 0; - int avgrx2_sum = 0; - //int avgry2_sum = 0; - - int vecrtime = 0; - - if(right_number>0) - for(VectorData point : right){ - int dx = Math.abs(point.x1 - point.x2); - //int dy = Math.abs(point.y1 - point.y2); - //float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); - if((dx/avgr)<5){ - avgrx1_sum += point.x1; - avgrx2_sum += point.x2; - //avgry1_sum += point.y1; - //avgry2_sum += point.y2; - vecrtime++; - } - } - - QuadrantFlowSpeed speed = new QuadrantFlowSpeed(); - - if(!(vecltime<=0 || vecrtime<=0)){ - int avgl_x1 = avglx1_sum / vecltime; - int avgl_x2 = avglx2_sum / vecltime; - //int avgl_y1 = avgly1_sum / vecltime; - //int avgl_y2 = avgly2_sum / vecltime; - - int avgr_x1 = avgrx1_sum / vecrtime; - int avgr_x2 = avgrx2_sum / vecrtime; - //int avgr_y1 = avgry1_sum / vecrtime; - //int avgr_y2 = avgry2_sum / vecrtime; - - int vec_l_x = avgl_x1 - avgl_x2; - //int vec_l_y = avgl_y1 - avgl_y2; - - int vec_r_x = avgr_x1 - avgr_x2; - //int vec_r_y = avgr_y1 - avgr_y2; - - speed.rx = vec_r_x; - //speed.ry = vec_r_y; - - speed.lx = vec_l_x; - //speed.ly = vec_l_y; - } - - //speed.tmp1 = tmp1; - //speed.tmp2 = tmp2; - - prev=curr; - + IplImage tmp_image = cvCreateImage(velikost, IPL_DEPTH_32F, 1); + + IntPointer corner_count = new IntPointer(1).put(MAX_CORNERS); + CvPoint2D32f cornersA = new CvPoint2D32f(MAX_CORNERS); + + CvArr mask = null; + cvGoodFeaturesToTrack(current, eig_image, tmp_image, cornersA, + corner_count, 0.01, 5.0, mask, 3, 0, 0.04); + + int win_size = 5; + + cvFindCornerSubPix(current, cornersA, corner_count.get(), + cvSize(win_size, win_size), cvSize(-1, -1), + cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); + + // Lucas Kande filter + + BytePointer features_found = new BytePointer(MAX_CORNERS); + FloatPointer feature_errors = new FloatPointer(MAX_CORNERS); + + CvSize pyr_sz = cvSize(current.width() + 8, previous.height() / 3); + + IplImage pyrA = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1); + IplImage pyrB = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1); + + CvPoint2D32f cornersB = new CvPoint2D32f(MAX_CORNERS); + cvCalcOpticalFlowPyrLK(current, previous, pyrA, pyrB, cornersA, + cornersB, corner_count.get(), cvSize(win_size, win_size), 5, + features_found, feature_errors, + cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3), 0); + + Vector left = new Vector(); + Vector right = new Vector(); + + float avgsum_left = 0; + float left_number = 0; + float avgsum_right = 0; + float right_number = 0; + + for (int i = 0; i < corner_count.get(); i++) { + if (features_found.get(i) == 0 || feature_errors.get(i) > 550) { + continue; + } + cornersA.position(i); + cornersB.position(i); + int ax = (int) cornersA.x(); + // int ay = (int) cornersA.y(); + int bx = (int) cornersB.x(); + // int by = (int) cornersB.y(); + if (ax < 100) { + left.add(new VectorData(ax, 0, bx, 0));// left.add(new + // VectorData(ax, ay, + // bx, by)); + int dx = Math.abs(ax - bx); + // int dy = Math.abs(ay-by); + // float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); + avgsum_left += dx; + left_number++; + } else if (ax > 220) { + right.add(new VectorData(ax, 0, bx, 0));// right.add(new + // VectorData(ax, ay, + // bx, by)); + int dx = Math.abs(ax - bx); + // int dy = Math.abs(ay-by); + // float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); + avgsum_right += dx; + right_number++; + } + } + + float avgl = avgsum_left / left_number; + float avgr = avgsum_right / right_number; + + int avglx1_sum = 0; + // int avgly1_sum = 0; + int avglx2_sum = 0; + // int avgly2_sum = 0; + + int vecltime = 0; + + if (left_number > 0) + for (VectorData point : left) { + int dx = Math.abs(point.x1 - point.x2); + // int dy = Math.abs(point.y1 - point.y2); + // float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); + if ((dx / avgl) < 5) { + avglx1_sum += point.x1; + avglx2_sum += point.x2; + // avgly1_sum += point.y1; + // avgly2_sum += point.y2; + vecltime++; + } + } + + int avgrx1_sum = 0; + // int avgry1_sum = 0; + int avgrx2_sum = 0; + // int avgry2_sum = 0; + + int vecrtime = 0; + + if (right_number > 0) + for (VectorData point : right) { + int dx = Math.abs(point.x1 - point.x2); + // int dy = Math.abs(point.y1 - point.y2); + // float length = (float) Math.sqrt(((dx*dx) + (dy*dy))); + if ((dx / avgr) < 5) { + avgrx1_sum += point.x1; + avgrx2_sum += point.x2; + // avgry1_sum += point.y1; + // avgry2_sum += point.y2; + vecrtime++; + } + } + + QuadrantFlowSpeed speed = new QuadrantFlowSpeed(); + + if (!(vecltime <= 0 || vecrtime <= 0)) { + int avgl_x1 = avglx1_sum / vecltime; + int avgl_x2 = avglx2_sum / vecltime; + // int avgl_y1 = avgly1_sum / vecltime; + // int avgl_y2 = avgly2_sum / vecltime; + + int avgr_x1 = avgrx1_sum / vecrtime; + int avgr_x2 = avgrx2_sum / vecrtime; + // int avgr_y1 = avgry1_sum / vecrtime; + // int avgr_y2 = avgry2_sum / vecrtime; + + int vec_l_x = avgl_x1 - avgl_x2; + // int vec_l_y = avgl_y1 - avgl_y2; + + int vec_r_x = avgr_x1 - avgr_x2; + // int vec_r_y = avgr_y1 - avgr_y2; + + speed.rx = vec_r_x; + // speed.ry = vec_r_y; + + speed.lx = vec_l_x; + // speed.ly = vec_l_y; + } + + // speed.tmp1 = tmp1; + // speed.tmp2 = tmp2; + + prev = curr; + return speed; } diff --git a/src/com/muhavision/cv/QuadrantFlowSpeed.java b/src/com/muhavision/cv/QuadrantFlowSpeed.java index 656573e..4708046 100644 --- a/src/com/muhavision/cv/QuadrantFlowSpeed.java +++ b/src/com/muhavision/cv/QuadrantFlowSpeed.java @@ -1,11 +1,7 @@ package com.muhavision.cv; -import java.util.Vector; - -import com.codeminders.ardrone.Point; - public class QuadrantFlowSpeed { - + public int rx, ry, lx, ly; } diff --git a/src/com/muhavision/cv/VectorData.java b/src/com/muhavision/cv/VectorData.java index 9e847da..fae5478 100644 --- a/src/com/muhavision/cv/VectorData.java +++ b/src/com/muhavision/cv/VectorData.java @@ -3,12 +3,12 @@ public class VectorData { public int x1, x2, y1, y2; - + public VectorData(int x1, int y1, int x2, int y2) { this.x1 = x1; this.x2 = x2; this.y1 = y1; this.y2 = y2; } - + } diff --git a/src/com/muhavision/cv/image/ImageData.java b/src/com/muhavision/cv/image/ImageData.java deleted file mode 100644 index 25f6dc2..0000000 --- a/src/com/muhavision/cv/image/ImageData.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.muhavision.cv.image; - -/** - * Created by ziga on 20.8.2014. - */ -public class ImageData { - - public int x, y, h, w; -} diff --git a/src/com/muhavision/cv/image/ImageHelper.java b/src/com/muhavision/cv/image/ImageHelper.java deleted file mode 100644 index fef6510..0000000 --- a/src/com/muhavision/cv/image/ImageHelper.java +++ /dev/null @@ -1,47 +0,0 @@ -package com.muhavision.cv.image; - -import java.awt.*; - -import javax.swing.JFrame; - -/** - * Created by ziga on 20.8.2014. - */ -public class ImageHelper { - - public static ImageData getScaledData(JFrame frame){ - Dimension dimension = frame.getSize(); - - - float h = 480; - float w = 640; - float sw = dimension.width; - float sh = dimension.height; - - float ratio = h / w; - float sratio = sh / sw; - - - ImageData result = new ImageData(); - - if(ratio == sratio){ - result.x = 0; - result.y = 0; - result.h = (int) sh; - result.w = (int) sw; - }else if (ratio > sratio){ - result.y = 0; - result.h = (int) sh; - result.w = Math.round((sh / h) * w); - result.x = Math.round((sw - result.w) / 2); - }else { - result.x = 0; - result.w = (int) sw; - result.h = Math.round((sw / w) * h); - result.y = Math.round((sh - result.h) / 2); - } - - return new ImageData(); - - } -} diff --git a/src/com/muhavision/cv/image/VisualRenderer.java b/src/com/muhavision/cv/image/VisualRenderer.java index 69d1728..835f95c 100644 --- a/src/com/muhavision/cv/image/VisualRenderer.java +++ b/src/com/muhavision/cv/image/VisualRenderer.java @@ -1,96 +1,89 @@ package com.muhavision.cv.image; -import java.awt.Color; import java.awt.Font; import java.awt.Graphics; -import java.awt.Graphics2D; -import java.awt.Toolkit; import java.awt.image.BufferedImage; import javax.swing.ImageIcon; -import javax.swing.JFrame; -import javax.swing.JLabel; import javax.swing.JPanel; import com.codeminders.ardrone.NavData; -import com.codeminders.ardrone.Point; import com.muhavision.Main; import com.muhavision.control.EulerAngles; import com.muhavision.control.FlightMode; import com.muhavision.cv.QuadrantFlowSpeed; -public class VisualRenderer extends JPanel{ - +public class VisualRenderer extends JPanel { + + /** + * + */ + private static final long serialVersionUID = 1L; public BufferedImage image = null; long prevData = 0; float avgFps; float filtT = 10; - - ImageData data = new ImageData(); + QuadrantFlowSpeed speed = new QuadrantFlowSpeed(); - + NavData fdata = null; EulerAngles angles = null; - + public Main global_main = null; - + public VisualRenderer(Main main) { - this.global_main = main; - } - - public void setDataProps(JFrame frame){ - data = ImageHelper.getScaledData(frame); + global_main = main; } - - public void reloadDatas(BufferedImage image, QuadrantFlowSpeed speed, NavData fdata, EulerAngles angles){ - this.image = image; - this.speed = speed; - this.fdata = fdata; - this.angles = angles; - repaint(); - } - - public void reloadNoData(){ - repaint(); - } - + @Override - public void paintComponent(Graphics g_panel){ - BufferedImage bimage = new BufferedImage(320, 240, BufferedImage.TYPE_INT_BGR); - + public void paintComponent(Graphics g_panel) { + BufferedImage bimage = new BufferedImage(320, 240, + BufferedImage.TYPE_INT_BGR); + Graphics g = bimage.getGraphics(); - - if(image!=null) + + if (image != null) g.drawImage(image, 0, 0, this); else - g.drawImage(new ImageIcon("./res/nosignal.png").getImage(), 0, 0, 320, 240, this); - + g.drawImage(new ImageIcon("./res/nosignal.png").getImage(), 0, 0, + 320, 240, this); + g.setFont(new Font("Arial", Font.PLAIN, 10)); - - if(fdata!=null){ - g.drawString("Battery: "+(int)fdata.getBattery(), 250, 30); + + if (fdata != null) { + g.drawString("Battery: " + fdata.getBattery(), 250, 30); } - if(angles!=null){ - g.drawString("Mark. dist.: "+(int)angles.dist, 250, 38); + if (angles != null) { + g.drawString("Mark. dist.: " + angles.dist, 250, 38); g.fillRect(angles.picX, angles.picY, 4, 4); } - - g.drawString("Roll: "+(int)global_main.roll, 30, 30); - g.drawString("Pitch: "+(int)global_main.pitch, 30, 38); - g.drawString("Yaw: "+(int)global_main.yaw, 30, 46); - g.drawString("Height: "+(int)global_main.height, 30, 54); + + g.drawString("Roll: " + (int) global_main.roll, 30, 30); + g.drawString("Pitch: " + (int) global_main.pitch, 30, 38); + g.drawString("Yaw: " + (int) global_main.yaw, 30, 46); + g.drawString("Height: " + (int) global_main.height, 30, 54); g.drawOval(156, 116, 5, 5); - - if(speed!=null){ - g.drawLine(156, 116, 156+(speed.rx-speed.lx), 116); + + if (speed != null) { + g.drawLine(156, 116, 156 + (speed.rx - speed.lx), 116); } - - if(global_main!=null){ - g.drawString(FlightMode.getFlightModeName(global_main.flightMode.getMode()), 138, 80); + + if (global_main != null) { + g.drawString(FlightMode.getFlightModeName(global_main.flightMode + .getMode()), 138, 80); } g_panel.drawImage(bimage, 0, 0, getWidth(), getHeight(), this); } + public void reloadDatas(BufferedImage image, QuadrantFlowSpeed speed, + NavData fdata, EulerAngles angles) { + this.image = image; + this.speed = speed; + this.fdata = fdata; + this.angles = angles; + repaint(); + } + } diff --git a/src/com/muhavision/pid/PID.java b/src/com/muhavision/pid/PID.java index f56be9b..02bdc71 100644 --- a/src/com/muhavision/pid/PID.java +++ b/src/com/muhavision/pid/PID.java @@ -11,15 +11,21 @@ //PID controller!!! public class PID { - //Some variables - float p,i,d; + public static interface Direction { + public static int NORMAL = 1; + public static int REVERSED = -1; + } + + // Some variables + float p, i, d; float err; long prevTime = -1; float prevErr; float integration; int direction = 1; + int imax; - + public PID(int p, int i, int d, int imax, int direction) { this.p = p; this.i = i; @@ -27,43 +33,38 @@ public PID(int p, int i, int d, int imax, int direction) { this.imax = imax; this.direction = direction; } - - public float calculate(float input, float setpoint){ - - if(prevTime!=-1){ - - float dt = ((float)(System.nanoTime() - prevTime))/1000000000.0f; + + public float calculate(float input, float setpoint) { + + if (prevTime != -1) { + + float dt = (System.nanoTime() - prevTime) / 1000000000.0f; prevTime = System.nanoTime(); - + float err = setpoint - input; - float diff = (err - prevErr)/dt; - integration += err*dt; - + float diff = (err - prevErr) / dt; + integration += err * dt; + prevErr = err; - - if(integration>imax) + + if (integration > imax) integration = imax; - if(integration<-imax) + if (integration < -imax) integration = -imax; - - return (err*p + integration*i + diff*d)*direction; - - }else{ + + return (err * p + integration * i + diff * d) * direction; + + } else { prevTime = System.nanoTime(); return 0; } } - - public void resetPID(){ + + public void resetPID() { prevErr = 0; integration = 0; prevTime = -1; } - - public static interface Direction { - public static int NORMAL = 1; - public static int REVERSED = -1; - } } diff --git a/src/tests/Test1.java b/src/tests/Test1.java index 4881ed1..f7e9b49 100644 --- a/src/tests/Test1.java +++ b/src/tests/Test1.java @@ -1,27 +1,25 @@ package tests; -import static org.bytedeco.javacpp.opencv_core.*; -import static org.bytedeco.javacpp.opencv_imgproc.*; -import static org.bytedeco.javacpp.opencv_highgui.*; +import static org.bytedeco.javacpp.opencv_highgui.cvLoadImage; +import org.bytedeco.javacpp.opencv_core.IplImage; import org.bytedeco.javacv.CanvasFrame; public class Test1 { - public static void main(String[] args) { - - //Load image img1 as IplImage - final IplImage image = cvLoadImage("img.png"); - - //create canvas frame named 'Demo' - final CanvasFrame canvas = new CanvasFrame("Demo"); - - //Show image in canvas frame - canvas.showImage(image); - - //This will close canvas frame on exit - canvas.setDefaultCloseOperation(javax.swing.JFrame.EXIT_ON_CLOSE); - } - + + // Load image img1 as IplImage + final IplImage image = cvLoadImage("img.png"); + + // create canvas frame named 'Demo' + final CanvasFrame canvas = new CanvasFrame("Demo"); + + // Show image in canvas frame + canvas.showImage(image); + + // This will close canvas frame on exit + canvas.setDefaultCloseOperation(javax.swing.JFrame.EXIT_ON_CLOSE); + } + } \ No newline at end of file