// Method
//
public static void Interpreter()
{
string linknumber = string.Empty;
string Item = string.Empty;
string ItemName = string.Empty;
int ItemType = 0;
int CmpPrmValue = 0;
int iLoopCounter = 1;
string[] saParameter; // var used for the cmp channel
int[] iParameter = new int[StaticUtilities.numberOfServos];
int[] iServoChannel = new int[StaticUtilities.numberOfServos];
uint[] uiMixWidth = new UInt32[StaticUtilities.numberOfServos];
int iPlaytime = 1;
int iComparisonRegister = 0;
int cmpMain = 0;
// playing = true;
int originnumber = Start;
while (playing)
{
if (!pausing)
{
// The first item is de Item that Start points to
Item = string.Format("Item{0}", originnumber);
ItemName = KHR_1HV_Motion[Item][StaticUtilities.ItemName];
Log.WriteLineMessage(string.Format("Play Item{0}, Name: {1}", originnumber, ItemName));
ItemType = Convert.ToInt32(KHR_1HV_Motion[Item][StaticUtilities.ItemType]);
saParameter = KHR_1HV_Motion[Item][StaticUtilities.ItemPrm].Split(',');
iPlaytime = Convert.ToInt32(saParameter[0]);
cmpMain = 0; // Needed for the jump to link
switch (ItemType)
{
case 0: // POS
for (int ii = 0; ii < StaticUtilities.numberOfServos; ii++)
{
// convert the current string array Prm to an integer Array
iParameter[ii] = Convert.ToInt32(saParameter[ii + 1]);
// 1 - 0 <=> Doesn't do anything
if (iParameter[ii] == 0)
{
// Set the mixwidth to off until something else
uiMixWidth[ii] = 0; //RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 2 - 1 ~ 16384 ~ 32767 <=> Servo relative angle (Range -16383 ~ 0 ~ +16384)
else if ((iParameter[ii] >= 1) && (iParameter[ii] <= 32767))
{
uiMixWidth[ii] = 0;
iServoChannel[ii] = iParameter[ii] - 16384;
}
// 3 - 32768 <=> TTL Output(L)
else if (iParameter[ii] == 32768)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 4 - 32769 <=> TTL Output (H)
else if (iParameter[ii] == 32770)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 5 - 32770 <=> Free
else if (iParameter[ii] == 32770)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 6 - 32771 <=> SET1(PWM only)
else if (iParameter[ii] == 32771)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 7 - 32772 <=> SET2(PWM only)
else if (iParameter[ii] == 32772)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
// 8 - 32773 <=> SET3(PWM only)
else if (iParameter[ii] == 32773)
{
uiMixWidth[ii] = RoBoIO_DotNet.RoBoIO.RCSERVO_MIXWIDTH_POWEROFF;
iServoChannel[ii] = 0;
}
} // END For loop
// Play the motion
Servos(iServoChannel, iPlaytime);
break;
case 1: // SET
// The SET command is usually only 1 Prm, the rest is the same.
// That's why there is no for loop here.
CmpPrmValue = Convert.ToInt32(saParameter[1]);
// 1 - Setup of a Loop counter 0 - 255 <=> 33287 ~ 33542
if ((CmpPrmValue >= 33287) && (CmpPrmValue <= 33542))
{
iLoopCounter = CmpPrmValue - 33287;
}
// 2 - Setup of the Comparison Register <=> 39942(-1023)~40964(-1), 40965(0)~57348(16383) <=> -1023~16383
else if ((CmpPrmValue >= 39942) && (CmpPrmValue <= 57348))
{
iComparisonRegister = CmpPrmValue - 40965;
}
// 3 - Calibration of AD1 standard value
else if (CmpPrmValue == 65268)
{
// Perform calibration of the sensor
}
// 4 - Calibration of AD2 standard value
else if (CmpPrmValue == 65269)
{
// Perform calibration of the sensor
}
// 5 - Calibration of AD3 standard value
else if (CmpPrmValue == 65270)
{
// Perform calibration of the sensor
}
// 6 - Jump to scenario
else if ((CmpPrmValue >= 57349) && (CmpPrmValue <= 57428))
{
}
// 7 - Call to scenario
else if ((CmpPrmValue >= 57434) && (CmpPrmValue <= 57513))
{
}
// 8 - Return from call
else if (CmpPrmValue == 57519)
{
}
break;
case 2: // CMP
CmpPrmValue = Convert.ToInt32(saParameter[1]);
// 1 - Decrement of a loop counter 33543 ~ 33798
if ((CmpPrmValue >= 33543) && (CmpPrmValue <= 33978))
{
if (--iLoopCounter != 0)
{
cmpMain = 1; // Find the link with the main key
}
}
// 2 - Comparison of AD1 value with register, if greater then jump
// 35846 ~ 36101
else if ((CmpPrmValue >= 35846) && (CmpPrmValue <= 36101))
{
// read a sensor value
// which sensor?
int sensorvalueAD1 = 100;
if (sensorvalueAD1 > iComparisonRegister)
{
cmpMain = 1; // Find the link with the main key
}
}
//=====================================================
// OMDAT DE VALUE MAX 100 IS KAN ONDERSTAANDE CMP
// AANGEPAST WORDEN ZODAT ER OOK RUIMTE IS VOOR DE
// LEFT EN RIGHT TRIGGERS.
// MAAR DIT MOET EERST IN DE EDITOR GEDAAN WORDEN.
//=====================================================
// 3 - Comparison of ThumbsticksX1 (PA1) Value with register, if greater then jump
// 37382 ~ 37637
else if ((CmpPrmValue >= 37382) && (CmpPrmValue <= 37637))
{
Server.XBox360.ControllerState();
if ((Server.XBox360.ThumbsticksX1 - MainIni.PA1Reference) > iComparisonRegister)
{
cmpMain = 1;
}
}
// 4 - Comparison of ThumbsticksY1 (PA2) Value with register, if greater then jump
else if ((CmpPrmValue >= 37638) && (CmpPrmValue <= 37893))
{
Server.XBox360.ControllerState();
if ((Server.XBox360.ThumbsticksY1 - MainIni.PA2Reference) > iComparisonRegister)
{
cmpMain = 1;
}
}
// 5 - Comparison of ThumbsticksY2 (PA3) Value with register, if greater then jump
else if ((CmpPrmValue >= 37894) && (CmpPrmValue <= 38149))
{
Server.XBox360.ControllerState();
if ((Server.XBox360.ThumbsticksX2 - MainIni.PA3Reference) > iComparisonRegister)
{
cmpMain = 1;
}
}
// 6 - Comparison of ThumbsticksX2 (PA4) Value with register, if greater then jump
else if ((CmpPrmValue >= 38150) && (CmpPrmValue <= 38405))
{
Server.XBox360.ControllerState();
if ((Server.XBox360.ThumbsticksY2 - MainIni.PA4Reference) > iComparisonRegister)
{
cmpMain = 1;
}
}
// 7 - Comparison of LeftTrigger (PA5) Value with register, if greater then jump
// 8 - Comparison of RightTrigger (PA6) Value with register, if greater then jump
break;
default:
break;
} // END Switch
// if there is a link find it, else stop playing.
bool LinkFound = false;
// find the next link and check if the main item
// is flow wiring or branch wiring
for (int j = 0; j < Links; j++)
{
string linkn = string.Format("{0}{1}", StaticUtilities.SectionLink, j);
int linkorg = Convert.ToInt32(KHR_1HV_Motion[linkn][StaticUtilities.LinkOrigin]);
int linkmain = Convert.ToInt32(KHR_1HV_Motion[linkn][StaticUtilities.LinkMain]);
if ((originnumber == linkorg) && (linkmain == cmpMain))
{
// make the origin the final
originnumber = Convert.ToInt32(KHR_1HV_Motion[linkn][StaticUtilities.LinkFinal]);
LinkFound = true;
break;
}
}
if (!LinkFound)
{
playing = false;
}
Server.XBox360.ControllerState();
if (Server.XBox360.Buttons == 128)
{
playing = false;
}
} // END while (!pausing)
} // END while (playing)
pausing = false;
Log.WriteLineMessage("Playing done");
Network.SendMessage("DonePlaying");
}
//
//
public static void mainServer_messageHandler(object sender, NewMessageEventsArgs e)
{
networkBusy = true;
Log.WriteLineMessage(e.NewMessage);
string sendMsg = string.Empty;
string[] message;
string[] Msg;
message = e.NewMessage.Split(','); // first index contains the command
// Make a command parser.
//
switch (message[0])
{
//
// Main Menu
case "Information":
sendMsg = string.Empty;
sendMsg = string.Format("{0},{1},{2},{3},{4},{5}",
RCServo.CPU(), RCServo.Version(), Convert.ToString(RCServo.Connected),
Convert.ToString(I2C.Connected),
Convert.ToString(AD7918.Connected),
Convert.ToString(SPI.Connected));
break;
case "Options":
switch (message[1])
{
case "Read":
string tmp = string.Empty;
sendMsg = Server.MainIni.MotionReplay.ToString() + "," +
Server.MainIni.EnableRemoteControl.ToString() + "," +
Server.MainIni.PowerUpMotion + "," +
Server.MainIni.LowPowerMotion + "," +
Server.MainIni.LowPowerVoltage.ToString() + "," +
Server.MainIni.TimeBase.ToString();
for (int i = 0; i < StaticUtilities.numberOfServos; i++)
{
tmp += string.Format(",{0}", Server.MainIni.ChannelFunction[i]);
}
sendMsg = sendMsg + tmp;
break;
case "Write":
Server.MainIni.MotionReplay = Convert.ToBoolean(message[2]);
Server.MainIni.EnableRemoteControl = Convert.ToBoolean(message[3]);
Server.MainIni.PowerUpMotion = Convert.ToInt32(message[4]);
Server.MainIni.LowPowerMotion = Convert.ToInt32(message[5]);
Server.MainIni.LowPowerVoltage = Convert.ToInt32(message[6]);
Server.MainIni.TimeBase = Convert.ToInt32(message[7]);
int[] temp = new int[StaticUtilities.numberOfServos];
for (int i = 0; i < StaticUtilities.numberOfServos; i++)
{
temp[i] = Convert.ToInt32(message[8 + i]);
}
Server.MainIni.ChannelFunction = temp;
Server.MainIni.Save();
Server.RCServo.Close();
Server.RCServo.Init();
if (MainIni.EnableRemoteControl)
{
if (!Server.XBox360.Open)
{
Server.XBox360.Init();
}
}
sendMsg = "Ok";
break;
default:
break;
}
break;
case "ReadMotionFile":
switch (message[1])
{
case "Open":
// Get the selected motion index and check
// in the motiontable if the motion exists
// return true if the file exist.
selectedMotionIndex = Convert.ToInt32(message[2]) + 1;
string file = Server.Table.MotionTable["Motion" + selectedMotionIndex.ToString()]["Filename"];
if (file != string.Empty)
{
khr_1hv_motion = new IniFile(file);
khr_1hv_motion.Load();
sendMsg = "Ok";
}
else
{
sendMsg = "No motion to read";
}
break;
case "GraphicalEdit":
Msg = new string[8];
Msg[0] = khr_1hv_motion["GraphicalEdit"]["Type"];
Msg[1] = khr_1hv_motion["GraphicalEdit"]["Width"];
Msg[2] = khr_1hv_motion["GraphicalEdit"]["Height"];
Msg[3] = khr_1hv_motion["GraphicalEdit"]["Items"];
Msg[4] = khr_1hv_motion["GraphicalEdit"]["Links"];
Msg[5] = khr_1hv_motion["GraphicalEdit"]["Start"];
Msg[6] = khr_1hv_motion["GraphicalEdit"]["Name"];
Msg[7] = khr_1hv_motion["GraphicalEdit"]["Ctrl"];
Items = 0;
Links = 0;
sendMsg = string.Join(",", Msg);
break;
case "Item":
Msg = new string[8];
string strItems = string.Format("Item{0}", Items++);
Msg[0] = khr_1hv_motion[strItems]["Name"];
Msg[1] = khr_1hv_motion[strItems]["Width"];
Msg[2] = khr_1hv_motion[strItems]["Height"];
Msg[3] = khr_1hv_motion[strItems]["Left"];
Msg[4] = khr_1hv_motion[strItems]["Top"];
Msg[5] = khr_1hv_motion[strItems]["Color"];
Msg[6] = khr_1hv_motion[strItems]["Type"];
Msg[7] = khr_1hv_motion[strItems]["Prm"];
sendMsg = string.Join(",", Msg);
break;
case "Link":
Msg = new string[4];
string strLinks = string.Format("Link{0}", Links++);
Msg[0] = khr_1hv_motion[strLinks]["Main"];
Msg[1] = khr_1hv_motion[strLinks]["Origin"];
Msg[2] = khr_1hv_motion[strLinks]["Final"];
Msg[3] = khr_1hv_motion[strLinks]["Point"];
sendMsg = string.Join(",", Msg);
break;
default:
break;
}
break;
case "WriteMotionFile":
switch (message[1])
{
case "Open":
// create random filename.
fileName = Path.ChangeExtension(Path.GetRandomFileName(), "RMF");
fileName = string.Format("{0}\\{1}", motionsFolder, fileName);
// get the index for the datatable where the
// motion is going to be writen.
selectedMotionIndex = Convert.ToInt32(message[2]) + 1;
khr_1hv_motion = new IniFile(fileName);
sendMsg = "Ok";
break;
case "GraphicalEdit":
IniSection section1 = new IniSection();
section1.Add("Type", message[2]);
section1.Add("Width", message[3]);
section1.Add("Height", message[4]);
section1.Add("Items", message[5]);
section1.Add("Links", message[6]);
section1.Add("Start", message[7]);
section1.Add("Name", message[8]);
section1.Add("Ctrl", message[9]);
khr_1hv_motion.Add("GraphicalEdit", section1);
Items = 0;
Links = 0;
sendMsg = "Ok";
break;
case "Item":
IniSection section2 = new IniSection();
section2.Add("Name", message[2]);
section2.Add("Width", message[3]);
section2.Add("Height", message[4]);
section2.Add("Left", message[5]);
section2.Add("Top", message[6]);
section2.Add("Color", message[7]);
section2.Add("Type", message[8]);
string strChannel = string.Format("{0}", message[9]);
for (int i = 10; i < message.Length; i++)
{
strChannel = string.Format("{0},{1}", strChannel, message[i]);
}
section2.Add("Prm", strChannel);
string strItems = string.Format("Item{0}", Items++);
khr_1hv_motion.Add(strItems, section2);
sendMsg = "Ok";
break;
case "Link":
IniSection section3 = new IniSection();
section3.Add("Main", message[2]);
section3.Add("Origin", message[3]);
section3.Add("Final", message[4]);
string strPoint = string.Format("{0}", message[5]);
for (int i = 6; i < message.Length; i++)
{
strPoint = string.Format("{0},{1}", strPoint, message[i]);
}
section3.Add("Point", strPoint);
string strLinks = string.Format("Link{0}", Links++);
khr_1hv_motion.Add(strLinks, section3);
sendMsg = "Ok";
break;
case "Save":
// get the filename of the motion to be deleted
string file = Server.Table.MotionTable["Motion" + selectedMotionIndex.ToString()]["Filename"];
if (file != string.Empty)
{
if (File.Exists(file))
{
File.Delete(file);
Log.WriteLineSucces(string.Format("Deleting: {0}", Server.Table.MotionTable["Motion" + selectedMotionIndex.ToString()]["Name"]));
}
else
{
Log.WriteLineFail(string.Format("Deleting: {0}", Server.Table.MotionTable["Motion" + selectedMotionIndex.ToString()]["Name"]));
}
}
Name = khr_1hv_motion["GraphicalEdit"]["Name"];
Ctrl = khr_1hv_motion["GraphicalEdit"]["Ctrl"];
if (!khr_1hv_motion.Save())
{
Log.WriteLineError(string.Format("Saving: {0}", Name));
sendMsg = "NOk";
}
else
{
Log.WriteLineSucces(string.Format("Saving: {0}", Name));
Server.Table.addNewMotion(selectedMotionIndex, fileName, Name, Ctrl);
Server.Table.Save();
sendMsg = "Ok";
}
break;
}
break;
case "PlayMotionFile":
int strMotion = (int.Parse(message[1]));
Log.WriteLineMessage(string.Format("Playing motion: {0}, {1}", Table.MotionTable["Motion" + (strMotion + 1).ToString()]["Name"], Table.MotionTable["Motion" + (strMotion + 1).ToString()]["Control"]));
Server.MotionInterpreter.Filename = Server.Table.MotionTable["Motion" + (strMotion + 1).ToString()]["Filename"];
Server.MotionInterpreter.Play();
//sendMsg = "Ok"; // No need for this.
break;
case "StopMotionFile":
Server.MotionInterpreter.Stop();
break;
case "PauseMotionFile":
Server.MotionInterpreter.Pause();
break;
case "DataTable":
switch (message[1])
{
case "Open":
sendMsg = string.Format("Open,{0},{1}", StaticUtilities.numberOfMotions, StaticUtilities.numberOfDataTableItems);
break;
case "Get":
string[] strTable = new string[StaticUtilities.numberOfDataTableItems];
string tmp = string.Format("{0}{1}", StaticUtilities.DataTableMotion, message[2]);
strTable[0] = Server.Table.MotionTable[tmp][StaticUtilities.DataTableMotion];
strTable[1] = Server.Table.MotionTable[tmp][StaticUtilities.DataTableName];
strTable[2] = Server.Table.MotionTable[tmp][StaticUtilities.DataTableCount];
strTable[3] = Server.Table.MotionTable[tmp][StaticUtilities.DataTableDate];
strTable[4] = Server.Table.MotionTable[tmp][StaticUtilities.DataTableCtrl];
sendMsg = string.Format("Get,{0}", string.Join(",", strTable));
break;
default:
break;
}
break;
case "DeleteMotionFile":
int strMotion1 = int.Parse(message[1]) + 1;
string file1 = Server.Table.MotionTable["Motion" + strMotion1.ToString()]["Filename"];
Server.Table.deleteMotion(strMotion1);
Server.Table.Save();
if (file1 != string.Empty)
{
if (File.Exists(file1))
{
File.Delete(file1);
Log.WriteLineSucces(string.Format("Deleting: {0}", file1));
}
else
{
Log.WriteLineFail(string.Format("Deleting: {0}", file1));
}
}
else
{
Log.WriteLineFail("Deleting: No such file");
}
// send something back as conformation.
sendMsg = "Ok";
break;
case "Servos":
switch (message[1])
{
case "Set":
int[] width = new int[StaticUtilities.numberOfServos];
Array.Copy(message, 2, strChannelData, 0, 24);
for (int q = 0; q < StaticUtilities.numberOfServos; q++)
{
width[q] = Convert.ToInt32(strChannelData[q]);
}
Server.MotionInterpreter.Servos(width, 100);
break;
default:
break;
}
break;
case "Trim":
switch (message[1])
{
case "Get":
sendMsg = Server.Trim.Data;
// Set the servo's position to the trim values
MotionInterpreter.Trim();
break;
case "Set":
Array.Copy(message, 2, strChannelData, 0, 24);
Server.Trim.Data = string.Join(",", strChannelData);
sendMsg = "Ok";
// misschien als check de trim.data weer terugsturen.
MotionInterpreter.Trim();
break;
case "Stop":
Server.Trim.Save();
sendMsg = "Ok";
break;
default:
break;
}
break;
case "Close":
case "close":
infinteLoop = false;
sendMsg = "No Connection";
break;
case "Open":
sendMsg = "Connected";
break;
case "XBox360Controller":
switch (message[1])
{
case "Open":
if (Server.XBox360.Open)
{
Server.XBox360.ControllerState();
}
sendMsg = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13}",
Server.XBox360.Open,
Server.XBox360.Buttons,
Server.XBox360.ThumbsticksX1, Server.XBox360.ThumbsticksY1,
Server.XBox360.ThumbsticksX2, Server.XBox360.ThumbsticksY2,
Server.XBox360.TriggerLeft, Server.XBox360.TriggerRight,
MainIni.PA1Reference, MainIni.PA2Reference, MainIni.PA3Reference, MainIni.PA4Reference,
MainIni.PA5Reference, MainIni.PA6Reference);
break;
case "Read":
Server.XBox360.ControllerState();
sendMsg = string.Format("{0},{1},{2},{3},{4},{5},{6},{7}",
Server.XBox360.Open,
Server.XBox360.Buttons,
Server.XBox360.ThumbsticksX1, Server.XBox360.ThumbsticksY1,
Server.XBox360.ThumbsticksX2, Server.XBox360.ThumbsticksY2,
Server.XBox360.TriggerLeft, Server.XBox360.TriggerRight);
break;
case "Stop":
MainIni.PA1Reference = Convert.ToInt32(message[2]);
MainIni.PA2Reference = Convert.ToInt32(message[3]);
MainIni.PA3Reference = Convert.ToInt32(message[4]);
MainIni.PA4Reference = Convert.ToInt32(message[5]);
MainIni.PA5Reference = Convert.ToInt32(message[6]);
MainIni.PA6Reference = Convert.ToInt32(message[7]);
MainIni.Save();
//sendMsg = "Ok";
break;
default:
break;
}
break;
case "MagnetoData":
short[] compass = Compass.CompassData();
sendMsg = "MagnetoData," +
compass[0].ToString() + "," +
compass[1].ToString() + "," +
compass[2].ToString();
break;
case "AcceleroData":
short[] accelero = Accelero.AcceleroData();
sendMsg = "AcceleroData," +
accelero[0].ToString() + "," +
accelero[1].ToString() + "," +
accelero[2].ToString();
break;
case "GyroscopeData":
short[] gyro = Gyro.GyroData();
sendMsg = "GyroscopeData," +
gyro[0].ToString() + "," +
gyro[1].ToString() + "," +
gyro[2].ToString() + "," +
gyro[3].ToString() + "," +
gyro[4].ToString() + "," +
gyro[5].ToString() + "," +
gyro[6].ToString() + "," +
gyro[7].ToString();
break;
default:
break;
}
if (sendMsg != string.Empty)
{
Network.SendMessage(sendMsg);
}
networkBusy = false;
}
