| 0,0 → 1,110 |
|---|
| 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); |
| } |
| } |