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