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;
}
public void registerBrowingFSM(RSV2BrowsingFSM bfsm)
{
browsingFSM = bfsm;
}
public void resetFSM(int robi, int robj, int orient, int dim)
{
// setting Last known position of the robosapien to -1, -1 (unregistered)
LastKnownRsv2Ipos = -1;
LastKnownRsv2Jpos = -1;
// clearing task queue
TaskQueueLength = 0;
// reconstructing the map
MapDim = dim;
Map = new char[MapDim][];
BlockProbability = new double[MapDim][];
VoidProbability = new double[MapDim][];
int i, j;
for (i = 0; i < MapDim; i++)
{
BlockProbability[i] = new double[MapDim];
VoidProbability[i] = new double[MapDim];
Map[i] = new char[MapDim];
for (j = 0; j < MapDim; j++)
{
Map[i][j] = cellUNKNOWN;
BlockProbability[i][j] = 0.5;
VoidProbability[i][j] = 0.5;
}
}
// assigning cart coordinates
ipos = robi;
jpos = robj;
orientation = orient;
// clearing robot cell
Map[ipos][jpos] = cellVOID;
// clear browsing fsm
browsingFSM = null;
// resetting state
state = stIdle;
// clearing completion flag
flagMappingDone = false;
}
// constructor
public MappingFSM(LMMobileRobot cart, int mapdim, int orient, int robi, int robj)
{
// setting Last known position of the robosapien to -1, -1 (unregistered)
LastKnownRsv2Ipos = -1;
LastKnownRsv2Jpos = -1;
// Setting displacement and angular error estimates to 0.
DisplacementX = DisplacementY = 0;
AngularError = 0;
// clearing number of correction tasks
CorrectionTaskNum = ExtraCorrectionTaskNum = 0;
// creating the Initial Sonar Array
InitialSonarArray = new ushort[8];
// copying constructor parameters
Cart = cart;
MapDim = mapdim;
// creating map filled with unknown cells
/*Map = new char[][] { new char[] {'*', '*', '*', '*', '*', '*', '*', '*'},
new char[] {'*', ' ', ' ', '*', ' ', '*', ' ', '*'},
new char[] {'*', ' ', ' ', '*', ' ', '*', ' ', '*'},
new char[] {'*', ' ', ' ', '*', ' ', ' ', ' ', '*'},
new char[] {'*', '*', ' ', ' ', ' ', '*', '*', '*'},
new char[] {'*', ' ', '*', ' ', '*', ' ', ' ', '*'},
new char[] {'*', ' ', ' ', ' ', ' ', ' ', '*', '*'},
new char[] {'*', '*', '*', '*', '*', '*', '*', '*'}}; */
Map = new char[MapDim][];
BlockProbability = new double[MapDim][];
VoidProbability = new double[MapDim][];
int i, j;
for (i = 0; i < MapDim; i++)
{
Map[i] = new char[MapDim];
BlockProbability[i] = new double[MapDim];
VoidProbability[i] = new double[MapDim];
for (j = 0; j < MapDim; j++)
{
// filling map line with unknown cells
Map[i][j] = cellUNKNOWN;
// setting block probability for each cell to 0.5 (max. entropy prior)
BlockProbability[i][j] = 0.5;
VoidProbability[i][j] = 0.5;
}
}
Map[robi][robj] = cellVOID;
// initializing task Queue
TaskQueue = new int[MAX_QUEUE_LENGTH];
TaskQueueLength = 0;
// robot position
ipos = robi;
jpos = robj;
orientation = orient;
previousOrientation = orient;
// marking current position as void.
Map[ipos][jpos] = cellVOID;
// setting block probability to 0 for the robot position
BlockProbability[ipos][jpos] = 0;
// mapping is not finished
flagMappingDone = false;
browsingFSM = null;
// state is idle
state = stIdle;
}
