Trajectory from gyro/accellerometer/compas with 9DOF IMU (Michal Zemko, Peter Svitok)

Zadanie

Trajectory from gyro/accellerometer/compas. Requirement: must use this part: 9DOF IMU

O súčiastke

V projekte sme mali využiť súčiastku 9DOF IMU a následne zo získaných dát vykresliť prejdenú trajektóriu robota.

Na doske 9DOF IMU sa nachádzajú nasledujúce komponenty:

• akcelerometer
• magnetometer
• gyroskop

Etapy

• 1. Oboznamánie sa s projektom
• 2. Naprogramovanie v Jave
• 3. Záverečné prípravy

1. etapa

V prvej etape sme dostali súčiastku a cez program Putty skúšali jej funkčnosť.

2. etapa

V druhej etape sme začali programovať aplikáciu na vykresľovanie pohybu súčiastky.

Program je naprogramovaný v Jave :

Zdrojový kód

Vykreslovanie

    static JFrame frame = new JFrame("Points");
	
	public class pokus extends JPanel{
		  public void paintComponent(Graphics g) {
		      //super.paintComponent(g);

		      Graphics2D g2d = (Graphics2D) g;

		      g2d.setColor(Color.black);

		      Dimension size = getSize();
		      Insets insets = getInsets();

		      int w =  size.width - insets.left - insets.right;
		      int h =  size.height - insets.top - insets.bottom;

		      Random r = new Random();

//		      int x = Math.abs(r.nextInt()) % w;
//		      int y = Math.abs(r.nextInt()) % h;
		      int x = data_new[0]-data_old[0];
		      int y = data_new[1]-data_old[1];
		      startx+=x;
		      starty+=y;
		      x=startx;
		      y=starty;
		      int x2 = 0;
		      int y2 = 0;
		      
		      if(((x>0)&&(x<800))&&((y>0)&&(y<600))){
			      array.add(x);
			      array.add(y);

			      for(int i=0;i<array.size();i+=2){
			        x=array.get(i).intValue();
			        y=array.get(i+1).intValue();
			        if(i>1){
				        x2=array.get(i-2).intValue();
				        y2=array.get(i-1).intValue();
				        g2d.drawLine(x, y, x2, y2);
			        }
			        g2d.drawLine(x, y, x, y);
			      	g2d.drawLine(x+1, y, x-1, y);	
			      	g2d.drawLine(x, y+1, x, y-1);		
			      }
		      }
		  }
	}	

Vytvorenie spojenia s čipom 9DOF Razor IMU

void connect(String portName) throws Exception {
		paint();
		
		CommPortIdentifier portIdentifier = CommPortIdentifier
				.getPortIdentifier(portName);
		if (portIdentifier.isCurrentlyOwned()) {
			System.out.println("Error: Port is currently in use");
		} else {
			CommPort commPort = portIdentifier.open(this.getClass().getName(),
					2000);

			if (commPort instanceof SerialPort) {
				SerialPort serialPort = (SerialPort) commPort;
				serialPort.setSerialPortParams(38400, SerialPort.DATABITS_8,
						SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);

				InputStream in = serialPort.getInputStream();
				OutputStream out = serialPort.getOutputStream();

				(new Thread(new SerialReader(in))).start();
				(new Thread(new SerialWriter(out))).start();

			} else {
				System.out
						.println("Error: Only serial ports are handled by this example.");
			}
		}
	}

Kontrola prijatých dát

public boolean isOK(String input) {
			int comma = 0;
			for (int i = 0; i < input.length(); i++) {
				if ((Character.isDigit(input.charAt(i)))||(input.charAt(i) == '$')||
					(input.charAt(i) == '#')||(input.charAt(i) == ',')||(input.charAt(i) == '-')) {
					if (input.charAt(i) == ',') {
						comma++;
					}
				}
				else {
					System.out.println("bad input");
					return false;
				}
			}
			if (comma != 10) {
				return false;
			}
			return true;
		}

3. etapa

V tretej etape sme pristúpili k testovaniu aplikácie a pozorovaniu vykreslených bodov do kresliacej plochy

Download

Trajectory from gyro/accellerometer/compas with 9DOF IMU