/// <summary>
/// Primary function used to query the belt configuration
/// </summary>
/// <param name="typeMsg">Type of query:
/// "QRY ALL\r" | "QRY RHY\r" | "QRY MAG\r" | "QRY MTR\r" | "QRY SPT\r"
/// | "QRY VER\r"</param>
/// <param name="timeout">Time allowed to recieve data on incoming
/// serial COM port before processing buffer</param>
/// <returns>error code resulting from Query command</returns>
private error_t Query(string typeMsg, int timeout)
{
error_t return_error = error_t.EMAX;
clearBuffer(qry_resp);
//Send the QRY command if ports are setup
try {
return_error = change_acmd_mode(acmd_mode_t.ACM_LRN);
if (return_error == error_t.ESUCCESS) {
// Send command with wait time for belt to respond back.
return_error = SerialPortWriteData(typeMsg, timeout);
//read responses from Serial_Port and place RSP lines into string array
if (return_error == error_t.ESUCCESS) {
string[] buffer;
//Removes the '\r\n' characters
char[] delimiters = new char[] { '\r', '\n' }; //, ' '
//CANNOT USE REGEX in CF 2.0
//buffer = Regex.Split(serialIn.MsgInBuffer, "\r\n");
buffer = ByteToAscii(serialIn.DataRecvBuffer()).Split(delimiters);
int rsp_index = 1;
// Formats buffer into string array so each RSP line is an element and removes "\r"
for (int i = 0; i < buffer.Length; i++) {
//Ignore non RSP lines
if (buffer[i].Length >= 3) {
if (buffer[i].Substring(0, 3).Equals("RSP")) {
qry_resp[rsp_index] = buffer[i];
rsp_index++;
}
else if (buffer[i].Substring(0, 3).Equals("DBG")) {
// Not yet implemented
//string[] split = buffer[i].Split(' ');
//sys_error = (error_t)split[1][0]; // get char at index 0
}
}
}
rsp_index--; // decrement from last increment to give count of elements
qry_resp[0] = rsp_index.ToString(); //count of elements
}
}
}
catch (Exception e) {
qry_resp[0] = "0";
return_error = error_t.EXCQRYCMD;
}
_dll_error = return_error;
return return_error;
}
/*
* Method switches the global mode
*/
private error_t change_glbl_mode(mode_t mode)
{
error_t return_error = error_t.EMAX;
if (_glbl_mode == mode) {
// already in requested mode, do nothing and no loss of performance
_belt_error = error_t.ESUCCESS; // already in requested mode
return_error = _belt_error;
}
else if (mode == mode_t.M_ACTIVE) {
//switch to activate command mode
return_error = SerialPortWriteData("BGN\n", MAX_RESPONSE_TIMEOUT);
if (return_error == error_t.ESUCCESS)
_glbl_mode = mode_t.M_ACTIVE;
}
else { //if (_glbl_mode == mode_t.M_ACTIVE)
// back to learning mode
byte[] returnState = { 0x30, 0x30 }; //should send Hex 0x30 0x30 = mode 0
return_error = SerialPortWriteData(returnState, MAX_RESPONSE_TIMEOUT);
if (return_error == error_t.ESUCCESS)
_glbl_mode = mode_t.M_LEARN;
}
_dll_error = return_error;
return return_error;
}
/*
* Function is used to get status from belt after each send or receive
*/
private void checkBeltStatus(acmd_mode_t acmd_mode)
{
error_t error = error_t.EMAX;
// .NET SerialPort.ReadLine() results in \r\n
// BUT YOU CAN ONLY SEE "\n" WHEN DIRECTLY LISTENING TO PORT
//if (serialIn.MsgInBuffer.Equals("STS 0\r\n") || serialIn.MsgInBuffer.Equals("53 54 53 20 30 0D 0A "))
if (acmd_mode == acmd_mode_t.ACM_LRN) {
char[] delimiters = new char[] { '\r', '\n', ' ' };
string[] split = ByteToAscii(serialIn.DataRecvBuffer()).Split(delimiters);
for (int i = 0; i < split.Length; i++) {
if (String.Equals(split[i], "STS")) {
byte[] errors = IntegerStrToByte(split[i + 1]); // gets first error code
error = (error_t)(errors[0]);
}
}
}
// Otherwise the error code is returned as a hex digit without '\n'
else if (acmd_mode == acmd_mode_t.ACM_VIB) {
error = (error_t)(serialIn.DataRecvBuffer()[0]);
}
else { ; }//do nothing
_belt_error = error;
}
/// <summary>
/// Function is used to STOP vibration of all motors on the haptic belt
/// </summary>
/// <returns>error code resulting from Stop Vibrate command</returns>
public error_t StopAll()
{
error_t return_error = error_t.EMAX;
int motorCount = this.getMotors(QueryType.PREVIOUS);
// Assumes that QRY has already occured in order to activate motors
if (motorCount != 0) {
for (byte i = 0; i < motorCount; i++) {
// sending rhythm_cycles = 0 stops motor
return_error = Vibrate_Motor(i, 0x0, 0x0, 0x0);
}
}
else {
return_error = _belt_error;
}
_dll_error = return_error;
return return_error;
}
/*
* This method is used to change the mode of the application
* as the firmware mode is changed. The application and the
* firmware modes must stay synchronized.
*/
private error_t change_acmd_mode(acmd_mode_t mode)
{
error_t return_error = error_t.EMAX;
if (_acmd_mode == mode) {
_belt_error = error_t.ESUCCESS; // already in requested mode
return_error = _belt_error;
}
else if (mode == acmd_mode_t.ACM_LRN) {
//return to learning mode
return_error = change_glbl_mode(mode_t.M_LEARN);
if (return_error == error_t.ESUCCESS) {
_acmd_mode = mode;
}
}
else { //(mode == acmd_mode_t.ACM_VIB || acmd_mode_t.ACM_SPT || acmd_mode_t.ACM_GCL)
return_error = change_glbl_mode(mode_t.M_ACTIVE);
if (return_error == error_t.ESUCCESS) {
_acmd_mode = mode;
}
}
_dll_error = return_error;
return return_error;
}
/// <summary>
/// Closes all COM ports in use for the wireless haptic belt.
/// </summary>
public error_t ClosePorts()
{
error_t error = error_t.COMPRTOPEN;
if (serialIn != null && serialOut != null) {
error = serialIn.ClosePort();
if (_portInName != _portOutName)
error = serialOut.ClosePort();
}
// Reset mode variables & state machine
_glbl_mode = mode_t.M_LEARN;
_acmd_mode = acmd_mode_t.ACM_LRN;
_dll_error = error;
return error;
}
/// <summary>
/// Function is used to STOP vibration of a specific motor on the haptic belt
/// </summary>
/// <param name="motor_number"></param>
/// <returns>error code resulting from Stop Vibrate command</returns>
public error_t Stop(byte motor_number)
{
error_t return_error = error_t.EMAX;
if (motor_number < 0) {
return_error = error_t.ENOMOTOR;
}
else {
// sending rhythm_cycles = 0 stops motor
return_error = Vibrate_Motor(motor_number, 0x0, 0x0, 0x0);
}
_dll_error = return_error;
return return_error;
}
/// <summary>
/// Reset haptic belt and driver to the default states if any erroneous
/// states occur in DLL or belt. This is an attempt to regain control
/// to a known state.
/// </summary>
/// <param name="resetComPorts">Set if you want to reset the connection</param>
/// <returns>error code resulting from the reset</returns>
public error_t ResetHapticBelt(bool resetComPorts)
{
//Reset Com Ports
if (resetComPorts) {
_dll_error = ClosePorts();
System.Threading.Thread.Sleep(1000); //must use Sleep for Windows
_dll_error = OpenPorts();
}
//Reset Belt's mode
_dll_error = change_acmd_mode(acmd_mode_t.ACM_LRN);
return _dll_error;
}
/// <summary>
/// Send binary data (byte array) to the outgoing serial COM port
/// </summary>
/// <param name="data"></param>
/// /// <param name="responseTimeout">Time allowed to recieve data on incoming
/// serial COM port before processing buffer</param>
/// <returns>error code resulting from sending the binary data</returns>
public error_t SerialPortWriteData(byte[] data, int responseTimeout)
{
error_t error = error_t.COMPRTWRITE;
if (!port_setup)
error = error_t.COMPRTSETUP;
else if (!serialOut.IsOpen())
error = error_t.COMPRTNOTOPEN;
else {
error = serialOut.WriteData(data);
if (error == error_t.ESUCCESS) {
//get response from belt
error = serialIn.ReceiveData(SerialPortManager.DataType.Hex, responseTimeout);
if (error == error_t.ESUCCESS) {
checkBeltStatus();
error = _belt_error;
}
}
}
_dll_error = error;
return error;
}
/// <summary>
/// This function returns the firmware version from the specified QRY operation
/// </summary>
public string getVersion(QueryType query_type)
{
string return_value = "";
error_t return_error = error_t.NOTFOUND;
// Query configuration data from belt
return_error = QuerySelect("QRY VER\r", query_type);
// Search and process data
if (return_error == error_t.ESUCCESS) {
return_error = error_t.NOTFOUND;
try {
for (int index = 1; index < qry_resp.Length; index++) {
if (qry_resp[index] != null) {
string[] split = qry_resp[index].Split(' ');
//put the values from the response into the return string
if (split[1].Equals("VER")) {
return_value = split[2];
return_error = error_t.ESUCCESS;
break; // exit loop
}
}
}
}
catch {
return_error = error_t.EXCDATASEARCH;
}
}
_dll_error = return_error;
return return_value;
}
/// <summary>
/// Opens the COM ports if they have already been setup using SetupPorts().
/// </summary>
public error_t OpenPorts()
{
error_t error = error_t.COMPRTNOTOPEN;
if (port_setup) {
// this.DataRecievedFxn is already Method = {Void MethodName()} from parent class
if (DataReceivedFxn != null) {
// serialIn.DataRecievedFxn becomes Method = {Void Invoke()}
serialIn.DataReceivedFxn += new SerialPortManager.DataRecievedHandler(DataReceivedFxn);
}
error = serialIn.OpenPort();
if (_portInName != _portOutName)
error = serialOut.OpenPort();
// Send Use HEX 0x0A for newline to signal to belt "End of Transmission"
// In case the belt is in an unknown byte count or state
// expect COM port read timeout since nothing should be returned
byte[] test = { 0x0A }; // == '\n'
error_t status = SerialPortWriteData(test, 10);
}
_dll_error = error;
return error;
}
/// <summary>
/// This function returns the motor count from the specified QRY operation
/// </summary>
public byte getMotors(QueryType query_type)
{
byte return_values = 0;
error_t return_error = error_t.NOTFOUND;
// Query configuration data from belt
return_error = QuerySelect("QRY MTR\r", query_type);
// Search and process data
if (return_error == error_t.ESUCCESS) {
return_error = error_t.NOTFOUND;
try { //Convert.ToInt16 can cause exception
for (int index = 1; index < qry_resp.Length; index++) {
if (qry_resp[index] != null) {
string[] split = qry_resp[index].Split(' ');
//put the values from the response into the return array
if (split[1].Equals("MTR")) {
string motor_count = split[2]; // gets motor number string
return_values = (byte)Convert.ToByte(motor_count);
return_error = error_t.ESUCCESS;
break; // exit loop
}
}
}
}
catch {
return_error = error_t.EXCDATASEARCH;
}
}
_dll_error = return_error;
return return_values;
}
/// <summary>
/// Accessor method that returns the error code message for a particular error code.
/// </summary>
public string getErrorMsg(error_t error_code)
{
return "Code: " + (int)error_code + " " + Constants.error_t_names[(int)error_code];
}
/// <summary>
///
/// </summary>
/// <returns>error code resulting from Erase All command</returns>
public error_t Erase_All()
{
error_t return_error = error_t.EMAX;
try {
return_error = change_acmd_mode(acmd_mode_t.ACM_LRN);
if (return_error == error_t.ESUCCESS) {
// Send command with wait time for belt to respond back.
return_error = SerialPortWriteData("ZAP 1 2 3\n", MAX_RESPONSE_TIMEOUT); // requires 3 arguments
}
}
catch {
qry_resp[0] = "0";
return_error = error_t.EXCZAPCMD;
}
_dll_error = return_error;
return return_error;
}
private error_t QuerySelect(string msg, QueryType query_type)
{
error_t return_error = error_t.NOTFOUND;
// Query configuration data from belt
if (query_type == QueryType.SINGLE)
return_error = Query(msg, MAX_RESPONSE_TIMEOUT);
else if (query_type == QueryType.ALL)
return_error = Query("QRY ALL\r", MAX_RESPONSE_TIMEOUT);
else // No query - DLL data structure already populated from recent QRY
return_error = error_t.ESUCCESS;
_dll_error = return_error;
return return_error;
}
/// <summary>
/// Initializes the Serial COM ports
/// </summary>
/// <param name="portInName">COM port number used for recieving data, ie "COM8"</param>
/// <param name="portOutName">COM port number used for sending data, ie "COM8"</param>
/// <param name="baud">baud rate of connection</param>
/// <param name="dBits">number of data bits to be sent at a time </param>
/// <param name="sBits">stop bits to be used (0, 1, 1.5, 2)</param>
/// <param name="par">parity bits to be used (None, Odd, Even, Mark, Space)</param>
/// <param name="timeout">specifies the default timeout for sending/recieving data</param>
/// <returns>error code resulting from COM port setup</returns>
public error_t SetupPorts(string portInName, string portOutName, string baud, string dBits, string sBits, string par, int timeout)
{
error_t error = error_t.COMPRTSETUP;
// validate parameters
if (portInName == null || portOutName == null || baud == null || dBits == null
|| sBits == null || par == null || timeout == 0) {
error = error_t.COMPRTINVALID;
}
_portInName = portInName;
_portOutName = portOutName;
try {
// Set default Port Manager parameters
serialIn = new SerialPortManager(portInName, baud, dBits, sBits, par, timeout);
serialIn.EchoBack = true;
if (portInName != portOutName) {
serialOut = new SerialPortManager(portOutName, baud, dBits, sBits, par, timeout);
}
else serialOut = serialIn;
port_setup = true;
error = error_t.ESUCCESS;
}
catch {
//error = error_t.COMPRTSETUP; // already default
}
_dll_error = error;
return error;
}
/// <summary>
/// Primary function used to vibrate motors - send a bit string command
/// </summary>
/// <returns>error code resulting from Vibrat Motor command</returns>
private error_t Vibrate_Motor(acmd_mode_t cmd_mode, byte motor, byte rhythm, byte magnitude, byte rhythm_cycles)
{
byte[] command_byte = { 0x00, 0x00 };
error_t return_error = error_t.EMAX;
// Validate parameters
if (motor < 0 || rhythm < 0 || rhythm > 8
|| magnitude < 0 || magnitude > 8
|| rhythm_cycles < 0 || rhythm_cycles > 7) {
return_error = error_t.EARG;
}
else {
byte mode = 0x0;
//byte motor = 0x0; // this is the first valid motor
//byte rhythm = 0x7; //rhy H =7
//byte magnitude = 0x0; //mag A = 0
//byte rhythm_cycles = 0x6;
// This equates to hex command < 01 36 >
try {
return_error = change_acmd_mode(cmd_mode);
if (return_error == error_t.ESUCCESS) {
mode = (byte)cmd_mode;
// Send mode first, it is the LSB of the first byte in firmware struct active_command_t
command_byte[0] = (byte)((mode << 4) | (motor & 0xf));
// Send rhythm first, it is the LSB of the firmware struct vibration_t
command_byte[1] = (byte)(((rhythm & 0x7) << 5)
| ((magnitude & 0x3) << 3)
| (rhythm_cycles & 0x7));
return_error = SerialPortWriteData(command_byte, MAX_RESPONSE_TIMEOUT);
// BROWN OUT HANDLING - Error from sending msg can return odd
// error code if belt is currently in ACM.LRN. Reasonable risk that
// this will not occur when main controller is running on battery.
// error = 0xFC = 252 is from brown out condition
if (return_error > error_t.NOTFOUND) {
return_error = ResetHapticBelt();//error_t.ELOWPOWER;
// proceed for expected behavior of readtimeout
if (return_error == error_t.ESUCCESS)
//Try again with recursive call
return_error = Vibrate_Motor(cmd_mode, motor, rhythm, magnitude, rhythm_cycles);
}
}
}
catch (Exception e) {
return_error = error_t.EXCVIBCMD;
}
}
_dll_error = return_error;
return return_error;
}
public string getErrorMsg(error_t status)
{
return wirelessBelt.getErrorMsg(status);
}
