Subversion Repositories Projects

Rev

Rev 1688 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1688 Rev 1689
1 
package dongfang.mkt.main;
 1 
package dongfang.mkt.main;
2 
 
 2 
 
3 
import java.io.FileInputStream;
 3 
import java.io.FileInputStream;
4 
import java.io.FileWriter;
 4 
import java.io.FileWriter;
5 
import java.io.IOException;
 5 
import java.io.IOException;
6 
import java.io.InputStream;
 6 
import java.io.InputStream;
7 
 
 7 
 
8 
import javax.xml.parsers.DocumentBuilder;
 8 
import javax.xml.parsers.DocumentBuilder;
9 
import javax.xml.parsers.DocumentBuilderFactory;
 9 
import javax.xml.parsers.DocumentBuilderFactory;
10 
import javax.xml.parsers.ParserConfigurationException;
 10 
import javax.xml.parsers.ParserConfigurationException;
11 
import javax.xml.xpath.XPath;
 11 
import javax.xml.xpath.XPath;
12 
import javax.xml.xpath.XPathConstants;
 12 
import javax.xml.xpath.XPathConstants;
13 
import javax.xml.xpath.XPathExpressionException;
 13 
import javax.xml.xpath.XPathExpressionException;
14 
import javax.xml.xpath.XPathFactory;
 14 
import javax.xml.xpath.XPathFactory;
15 
 
 15 
 
16 
import org.w3c.dom.Document;
 16 
import org.w3c.dom.Document;
17 
import org.w3c.dom.Element;
 17 
import org.w3c.dom.Element;
18 
import org.w3c.dom.NodeList;
 18 
import org.w3c.dom.NodeList;
19 
import org.xml.sax.SAXException;
 19 
import org.xml.sax.SAXException;
20 
 
 20 
 
21 
import dongfang.mkt.comm.FrameQueue;
 21 
import dongfang.mkt.comm.FrameQueue;
22 
import dongfang.mkt.comm.MKConnection;
 22 
import dongfang.mkt.comm.MKConnection;
23 
import dongfang.mkt.comm.serial.RXTXSerialPort;
 23 
import dongfang.mkt.comm.serial.RXTXSerialPort;
24 
import dongfang.mkt.configuration.IMUConfiguration;
 24 
import dongfang.mkt.configuration.IMUConfiguration;
25 
import dongfang.mkt.configuration.MotorMixer;
 25 
import dongfang.mkt.configuration.MotorMixer;
26 
import dongfang.mkt.frames.ReadMotorMixerRequestFrame;
 26 
import dongfang.mkt.frames.ReadMotorMixerRequestFrame;
27 
import dongfang.mkt.frames.ReadMotorMixerResponseFrame;
 27 
import dongfang.mkt.frames.ReadMotorMixerResponseFrame;
28 
import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame;
 28 
import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame;
29 
import dongfang.mkt.frames.WriteMotorMixerRequestFrame;
 29 
import dongfang.mkt.frames.WriteMotorMixerRequestFrame;
30 
import dongfang.mkt.frames.WriteMotorMixerResponseFrame;
 30 
import dongfang.mkt.frames.WriteMotorMixerResponseFrame;
31 
 
 31 
 
32 
public class MotorMixerConfigurator {
 32 
public class MotorMixerConfigurator {
33 
        private static void configure(String portIdentifier, WriteMotorMixerRequestFrame frame) throws IOException {
 33 
        private static void configure(String portIdentifier, WriteMotorMixerRequestFrame frame) throws IOException {
34 
                MKConnection port = new RXTXSerialPort();
 34 
                MKConnection port = new RXTXSerialPort();
35 
                port.init(portIdentifier);
 35 
                port.init(portIdentifier);
36 
                FrameQueue q = new FrameQueue(port);
 36 
                FrameQueue q = new FrameQueue(port);
37 
 
 37 
 
38 
                q.sendRequest(frame);
 38 
                q.sendRequest(frame);
39 
                WriteMotorMixerResponseFrame r = (WriteMotorMixerResponseFrame) q.getResponseFor(frame, 5000);
 39 
                WriteMotorMixerResponseFrame r = (WriteMotorMixerResponseFrame) q.getResponseFor(frame, 5000);
40 
                if (r == null) {
 40 
                if (r == null) {
41 
                        System.err.println("ERROR. Timeout waiting for response.");
 41 
                        System.err.println("ERROR. Timeout waiting for response.");
42 
                } else if (!r.wasAccepted()) {
 42 
                } else if (!r.wasAccepted()) {
43 
                        System.err
 43 
                        System.err
44 
                                        .println("ERROR. Motor mixer not accepted. Check version against MK firmware EEPROM mixer version.");
 44 
                                        .println("ERROR. Motor mixer not accepted. Check version against MK firmware EEPROM mixer version.");
45 
                } else {
 45 
                } else {
46 
                        System.out.println("Saved motor mixer.");
 46 
                        System.out.println("Saved motor mixer.");
47 
                }
 47 
                }
48 
 
 48 
 
49 
                q.kill();
 49 
                q.kill();
50 
        }
 50 
        }
51 
 
 51 
 
52 
        private static void writeConfiguration(String portIdentifier, String fileName) throws IOException {
 52 
        private static void writeConfiguration(String portIdentifier, String fileName) throws IOException {
53 
                System.out.println("Writing motor mixer from file: " + fileName);
 53 
                System.out.println("Writing motor mixer from file: " + fileName);
54 
                InputStream inputStream = new FileInputStream(fileName);
 54 
                InputStream inputStream = new FileInputStream(fileName);
55 
                MotorMixer mm = new MotorMixer();
 55 
                MotorMixer mm = new MotorMixer();
56 
                mm.parseXMLMotorMixer(inputStream);
 56 
                mm.parseXML(inputStream);
- 
 
 57 
                char[] name = new char[12];
- 
 
 58 
                mm.getName().getChars(0, mm.getName().length(), name, 0);
57 
                WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, mm.toBinary());
 59 
                WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, name, mm.toBinary());
- 
 
 60 
 
58 
                configure(portIdentifier, frame);
 61 
                configure(portIdentifier, frame);
59 
        }
 62 
        }
60 
 
 63 
 
61 
        private static MotorMixer readMotorMixer(String portIdentifier) throws IOException {
 64 
        private static MotorMixer readMotorMixer(String portIdentifier) throws IOException {
62 
                MKConnection port =  new RXTXSerialPort();
 65 
                MKConnection port =  new RXTXSerialPort();
63 
                port.init(portIdentifier);
 66 
                port.init(portIdentifier);
64 
                FrameQueue q = new FrameQueue(port);
 67 
                FrameQueue q = new FrameQueue(port);
65 
                MotorMixer mm = new MotorMixer();
 68 
                MotorMixer mm = new MotorMixer();
66 
 
 69 
 
67 
                ReadMotorMixerRequestFrame frame = new ReadMotorMixerRequestFrame();
 70 
                ReadMotorMixerRequestFrame frame = new ReadMotorMixerRequestFrame();
68 
                q.sendRequest(frame);
 71 
                q.sendRequest(frame);
69 
                ReadMotorMixerResponseFrame r = (ReadMotorMixerResponseFrame) q.getResponseFor(frame, 5000);
 72 
                ReadMotorMixerResponseFrame r = (ReadMotorMixerResponseFrame) q.getResponseFor(frame, 5000);
70 
                if (r == null) {
 73 
                if (r == null) {
71 
                        System.err.println("ERROR. Timeout waiting for response.");
 74 
                        System.err.println("ERROR. Timeout waiting for response.");
72 
                } else {
 75 
                } else {
73 
                        int[][] matrix = r.getMatrix();
 76 
                        mm.setMatrix(r.getMatrix());
74 
                        mm.setMatrix(matrix);
 77 
                        char[] name = r.getName();
75 
                        System.out.println(mm.toXML());
 78 
                        int firstNulPos = name.length;
- 
 
 79 
                        for (int i=0; i<name.length; i++) {
- 
 
 80 
                                if (name[i] == 0) {
- 
 
 81 
                                        firstNulPos = i;
- 
 
 82 
                                        break;
- 
 
 83 
                                }
- 
 
 84 
                        }
- 
 
 85 
                        String s_name = new String(name, 0, firstNulPos);
- 
 
 86 
                        mm.setName(s_name);
76 
                }
 87 
                }
77 
                q.kill();
 88 
                q.kill();
78 
                return mm;
 89 
                return mm;
79 
        }
 90 
        }
80 
 
 91 
 
81 
        private static void readMotorMixer(String portIdentifier, String fileName) throws IOException {
 92 
        private static void readMotorMixer(String portIdentifier, String fileName) throws IOException {
82 
                MotorMixer mm = readMotorMixer(portIdentifier);
 93 
                MotorMixer mm = readMotorMixer(portIdentifier);
83 
                if (mm != null) {
 94 
                if (mm != null) {
- 
 
 95 
                        if (fileName != null) {
84 
                        FileWriter fw = new FileWriter(fileName);
 96 
                                FileWriter fw = new FileWriter(fileName);
85 
                        fw.write(mm.toXML());
 97 
                                fw.write(mm.toXML());
86 
                        fw.close();
 98 
                                fw.close();
- 
 
 99 
                        }
- 
 
 100 
                        System.out.println(mm.toXML());
87 
                }
 101 
                }
88 
        }
 102 
        }
89 
 
 103 
 
90 
        static void help() {
 104 
        static void help() {
91 
                System.err.println("Usage: MotorMixerConfigurator r [filename to write to]");
 105 
                System.err.println("Usage: MotorMixerConfigurator r <filename to write to>");
92 
                System.err.println("Usage: MotorMixerConfigurator w [filename to read from]");
 106 
                System.err.println("Usage: MotorMixerConfigurator w [filename to read from]");
93 
                System.exit(-1);
 107 
                System.exit(-1);
94 
        }
 108 
        }
95 
       
 109 
       
96 
        public static void main(String[] args) throws IOException {
 110 
        public static void main(String[] args) throws IOException {
- 
 
 111 
                if (args.length < 1) help();
97 
                if (!"r".equals(args[0]) && !"w".equals(args[0])) help();
 112 
                if (!"r".equals(args[0]) && !"w".equals(args[0])) help();
98 
                if ("w".equals(args[0]) && (args.length!=2)) help();
 113 
                if ("w".equals(args[0]) && args.length < 2) help();
99 
                if ("r".equals(args[0]) && (args.length!=2)) help();
 - 
 
100 
 
 114 
 
101 
                String portIdentifier = null;
 115 
                String portIdentifier = null;
102 
               
 116 
               
103 
                if ("r".equals(args[0])) {
 117 
                if ("r".equals(args[0])) {
104 
                        readMotorMixer(portIdentifier, args[1]);
 118 
                        readMotorMixer(portIdentifier, args.length < 2 ? null : args[1]);
105 
                } else {
 119 
                } else {
106 
                        writeConfiguration(portIdentifier, args[1]);
 120 
                        writeConfiguration(portIdentifier, args[1]);
107 
                }
 121 
                }
108 
                System.exit(0);
 122 
                System.exit(0);
109 
        }
 123 
        }
110 
}
 124 
}