/// <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;
}
/*
* 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;
}
/*
* 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>
/// 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;
}
