public Form1()
{
ModeCartTraining = ModeRSV2Training = false;
ModeCartExecutingMetaNetwork = false;
ModeCartMapping = ModeRSV2Browsing = false;
ModeRsv2ExecutingMetaNetwork = false;
InitializeComponent();
server = new EZRoboNetDevice(1, EZRoboNetDevice.t_Node_Station);
rnd = new Random((int)DateTime.Now.Ticks);
SensorDataRequested = CameraFrameRequested = false;
rsv2pass = cartpass = 0;
robosapien = new RobosapienV2(server, 2, rnd);
cart = new LMMobileRobot(server, 3, rnd);
cartTrainingFSM = new CartTrainingFSM(cart);
cartMNExecutingFSM = new CartMetaNetworkFSM(cart);
rsv2MNExecutingFSM = new RSV2MetanetworkFSM(robosapien);
// creating mapping and browsing FSMs
mappingFSM = new MappingFSM(cart, 4, MappingFSM.orSOUTH, 1, 1);
rsv2BrowsingFSM = new RSV2BrowsingFSM(mappingFSM, robosapien, 0, 1, RSV2BrowsingFSM.orSOUTH);
rsv2TrainingFSM = new RSV2TrainingFSM(robosapien);
timer1.Start();
timer2.Start();
comboBox1.SelectedIndex = 2;
comboBox2.SelectedIndex = 2;
}
// constructor
public RSV2BrowsingFSM(MappingFSM mapfsm, RobosapienV2 robo,
int robIpos, int robJpos, int robOrient)
{
mappingFSM = mapfsm;
Robosapien = robo;
ipos = robIpos;
jpos = robJpos;
orientation = robOrient;
LastKnownCartIpos = mappingFSM.ipos;
LastKnownCartJpos = mappingFSM.jpos;
// creating the task queue
TaskQueue = new int[MAX_QUEUE_LENGTH];
TaskQueueLength = 0;
// registering browsing FSM in mapping FSM
mappingFSM.registerBrowingFSM(this);
// now setting up our Last Known location inside the mappingh FSM
mappingFSM.LastKnownRsv2Ipos = robIpos;
mappingFSM.LastKnownRsv2Jpos = robJpos;
}
public static void drawMap(MappingFSM mapFSM, System.Windows.Forms.Panel panel)
{
int xdim = mapFSM.MapDim;
int ydim = mapFSM.MapDim;
int mpanelxdim = panel.Width;
int mpanelydim = panel.Height;
Graphics mg = panel.CreateGraphics();
mg.Clear(System.Drawing.Color.White);
// now calculating cell width and height
int cellwidth = (int)(mpanelxdim / xdim);
int cellheight = (int)(mpanelydim / ydim);
// cell height-width done
// now drawing maze (with robot)
int i, j;
for (i = 0; i < ydim; i++)
for (j = 0; j < xdim; j++)
{
int x1, y1, x2 = 0, y2 = 0;
x1 = j * cellwidth;
y1 = i * cellheight;
if (mapFSM.Map[i][j] != MappingFSM.cellVOID)
{
SolidBrush bluebrush = new SolidBrush(mapFSM.Map[i][j] == MappingFSM.cellBLOCK ? Color.Blue : Color.Gray);
System.Drawing.Pen blackpen = new Pen(System.Drawing.Color.Black);
mg.FillRectangle(bluebrush, x1, y1, cellwidth, cellheight);
mg.DrawRectangle(blackpen, x1, y1, cellwidth, cellheight);
blackpen.Dispose();
bluebrush.Dispose();
}
}
// now drawing the movile robot
MappingFSM.drawRobot(mapFSM, mg, mapFSM.jpos, mapFSM.ipos, mapFSM.orientation,
panel, Color.Yellow);
// now drawing the other robot (if any)
if (mapFSM.browsingFSM != null)
MappingFSM.drawRobot(mapFSM, mg, mapFSM.browsingFSM.jpos, mapFSM.browsingFSM.ipos,
mapFSM.browsingFSM.orientation, panel, Color.Green);
}
public static void drawRobot(MappingFSM mapFSM, Graphics mg,
int robotx, int roboty, int robotorient,
System.Windows.Forms.Panel panel,
Color color)
{
int xdim = mapFSM.MapDim;
int ydim = mapFSM.MapDim;
int mpanelxdim = panel.Width;
int mpanelydim = panel.Height;
int cellwidth = (int)(mpanelxdim / xdim);
int cellheight = (int)(mpanelydim / ydim);
int x1 = robotx * cellwidth;
int y1 = roboty * cellheight;
int x2 = 0, y2 = 0;
SolidBrush robobrush = new SolidBrush(color);
mg.FillEllipse(robobrush, x1, y1, cellwidth, cellheight);
robobrush.Dispose();
// denoting orientation
System.Drawing.Pen redpen = new Pen(System.Drawing.Color.Red);
switch (robotorient)
{
case MappingFSM.orNORTH: x1 = robotx * cellwidth + (int)(cellwidth / 2);
y1 = roboty * cellheight + (int)(cellheight / 2);
x2 = robotx * cellwidth + (int)(cellwidth / 2);
y2 = roboty * cellheight;
break;
case MappingFSM.orEAST: x1 = robotx * cellwidth + (int)(cellwidth / 2);
y1 = roboty * cellheight + (int)(cellheight / 2);
x2 = (robotx + 1) * cellwidth;
y2 = roboty * cellheight + (int)(cellheight / 2);
break;
case MappingFSM.orSOUTH: x1 = robotx * cellwidth + (int)(cellwidth / 2);
y1 = roboty * cellheight + (int)(cellheight / 2);
x2 = robotx * cellwidth + (int)(cellwidth / 2);
y2 = (roboty + 1) * cellheight;
break;
case MappingFSM.orWEST: x1 = robotx * cellwidth + (int)(cellwidth / 2);
y1 = roboty * cellheight + (int)(cellheight / 2);
x2 = robotx * cellwidth;
y2 = roboty * cellheight + (int)(cellheight / 2);
break;
}
mg.DrawLine(redpen, x1, y1, x2, y2);
redpen.Dispose();
}
public static void deleteRobot(MappingFSM mapFSM, int robotx, int roboty, Graphics mg,
System.Windows.Forms.Panel panel)
{
int xdim = mapFSM.MapDim;
int ydim = mapFSM.MapDim;
int mpanelxdim = panel.Width;
int mpanelydim = panel.Height;
int cellwidth = (int)(mpanelxdim / xdim);
int cellheight = (int)(mpanelydim / ydim);
int x1 = robotx * cellwidth;
int y1 = roboty * cellheight;
int x2 = 0, y2 = 0;
SolidBrush whitebrush = new SolidBrush(Color.White);
mg.FillEllipse(whitebrush, x1, y1, cellwidth, cellheight);
whitebrush.Dispose();
}