Subversion Repositories Projects

package dongfang.mkt.main;

import java.io.FileInputStream;

import java.io.FileWriter;

import java.io.IOException;

import java.io.InputStream;

import javax.xml.parsers.DocumentBuilder;

import javax.xml.parsers.DocumentBuilderFactory;

import javax.xml.parsers.ParserConfigurationException;

import javax.xml.xpath.XPath;

import javax.xml.xpath.XPathConstants;

import javax.xml.xpath.XPathExpressionException;

import javax.xml.xpath.XPathFactory;

import org.w3c.dom.Document;

import org.w3c.dom.Element;

import org.w3c.dom.NodeList;

import org.xml.sax.SAXException;

import dongfang.mkt.comm.FrameQueue;

import dongfang.mkt.comm.MKConnection;

import dongfang.mkt.comm.serial.RXTXSerialPort;

import dongfang.mkt.configuration.IMUConfiguration;

import dongfang.mkt.configuration.MotorMixer;

import dongfang.mkt.frames.ReadMotorMixerRequestFrame;

import dongfang.mkt.frames.ReadMotorMixerResponseFrame;

import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame;

import dongfang.mkt.frames.WriteMotorMixerRequestFrame;

import dongfang.mkt.frames.WriteMotorMixerResponseFrame;

public class MotorMixerConfigurator {

        private static void configure(String portIdentifier, WriteMotorMixerRequestFrame frame) throws IOException {

                MKConnection port = new RXTXSerialPort();

                port.init(portIdentifier);

                FrameQueue q = new FrameQueue(port);

                q.sendRequest(frame);

                WriteMotorMixerResponseFrame r = (WriteMotorMixerResponseFrame) q.getResponseFor(frame, 5000);

                if (r == null) {

                        System.err.println("ERROR. Timeout waiting for response.");

                } else if (!r.wasAccepted()) {

                        System.err

                                        .println("ERROR. Motor mixer not accepted. Check version against MK firmware EEPROM mixer version.");

                } else {

                        System.out.println("Saved motor mixer.");

                }

                q.kill();

        }

        private static void writeConfiguration(String portIdentifier, String fileName) throws IOException {

                System.out.println("Writing motor mixer from file: " + fileName);

                InputStream inputStream = new FileInputStream(fileName);

                MotorMixer mm = new MotorMixer();

                mm.parseXMLMotorMixer(inputStream);

                WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, mm.toBinary());

                configure(portIdentifier, frame);

        }

        private static MotorMixer readMotorMixer(String portIdentifier) throws IOException {

                MKConnection port =  new RXTXSerialPort();

                port.init(portIdentifier);

                FrameQueue q = new FrameQueue(port);

                MotorMixer mm = new MotorMixer();

                ReadMotorMixerRequestFrame frame = new ReadMotorMixerRequestFrame();

                q.sendRequest(frame);

                ReadMotorMixerResponseFrame r = (ReadMotorMixerResponseFrame) q.getResponseFor(frame, 5000);

                if (r == null) {

                        System.err.println("ERROR. Timeout waiting for response.");

                } else {

                        int[][] matrix = r.getMatrix();

                        mm.setMatrix(matrix);

                        System.out.println(mm.toXML());

                }

                q.kill();

                return mm;

        }

        private static void readMotorMixer(String portIdentifier, String fileName) throws IOException {

                MotorMixer mm = readMotorMixer(portIdentifier);

                if (mm != null) {

                        FileWriter fw = new FileWriter(fileName);

                        fw.write(mm.toXML());

                        fw.close();

                }

        }

        static void help() {

                System.err.println("Usage: MotorMixerConfigurator r [filename to write to]");

                System.err.println("Usage: MotorMixerConfigurator w [filename to read from]");

                System.exit(-1);

        }

        public static void main(String[] args) throws IOException {

                if (!"r".equals(args[0]) && !"w".equals(args[0])) help();

                if ("w".equals(args[0]) && (args.length!=2)) help();

                if ("r".equals(args[0]) && (args.length!=2)) help();

                String portIdentifier = null;

                if ("r".equals(args[0])) {

                        readMotorMixer(portIdentifier, args[1]);

                } else {

                        writeConfiguration(portIdentifier, args[1]);

                }

                System.exit(0);

        }

}