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.parseXML(inputStream);

                char[] name = new char[12];

                mm.getName().getChars(0, mm.getName().length(), name, 0);

                WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, name, 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 {

                        mm.setMatrix(r.getMatrix());

                        char[] name = r.getName();

                        int firstNulPos = name.length;

                        for (int i=0; i<name.length; i++) {

                                if (name[i] == 0) {

                                        firstNulPos = i;

                                        break;

                                }

                        }

                        String s_name = new String(name, 0, firstNulPos);

                        mm.setName(s_name);

                }

                q.kill();

                return mm;

        }

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

                MotorMixer mm = readMotorMixer(portIdentifier);

                if (mm != null) {

                        if (fileName != null) {

                                FileWriter fw = new FileWriter(fileName);

                                fw.write(mm.toXML());

                                fw.close();

                        }

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

                }

        }

        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 (args.length < 1) help();

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

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

                String portIdentifier = null;

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

                        readMotorMixer(portIdentifier, args.length < 2 ? null : args[1]);

                } else {

                        writeConfiguration(portIdentifier, args[1]);

                }

                System.exit(0);

        }

}