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); |
} |
} |