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;
}
//Training FSM constructor
public CartTrainingFSM(LMMobileRobot cart)
{
Cart = cart;
state = stIdle;
}
//Training FSM constructor
public CartMetaNetworkFSM(LMMobileRobot cart)
{
Cart = cart;
state = stIdle;
}
//Training FSM constructor
public CartMetaNetworkFSM(LMMobileRobot cart)
{
Cart = cart;
state = stIdle;
}
// finds an estimate on the robots position gradient based on the sonar readings
public static double[] getGradient(LMMobileRobot thecart, int curOrientation, int prevOrientation)
{
double[] gradient = new double[] { 0, 0 };
double D1x, D2x, D3x, D4x, D5x, D6x;
double D1y, D2y, D3y, D4y, D5y, D6y;
double[] Snew = new double[8];
double[] Sprev = new double[8];
int i;
// storing sensor index permutations for both new and previous orientations
int[] prevPerm = getSensorPermutation(prevOrientation);
int[] newPerm = getSensorPermutation(curOrientation);
for (i = 0; i < 8; i++)
{
Snew[i] = (double)thecart.SonarArray[newPerm[i]];
Sprev[i] = (double)thecart.PrevSonarArray[prevPerm[i]];
}
// initially applying a Kalman Filter to the measurements
// a zero-input vector for the process
double[] U = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] B = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] W0 = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] V0 = new double[] { 2, 2, 2, 2, 2, 2, 2, 2 };
double[][] A = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] H = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] Q = new double[][] { new double[] { 5, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 5, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 5, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 5, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 5, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 5, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 5, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 5 }
};
double[][] R = new double[][] { new double[] { 2, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 2, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 2, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 2, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 2, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 2, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 2, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 2 }
};
KalmanFilter filter = new KalmanFilter(8, A, B, W0, H, V0, Q, R, Sprev);
// retrieving the measurement noise-free estimate by the Kalman filter,
// in order to use it to calculate the gradient
double[] Snest = filter.getNewEstimate(Snew, U);
// computing Gx
D1x = Sprev[1] * Math.Cos(Math.PI / 4) - Snest[1] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D1x) > sup) D1x = 0;
D2x = Sprev[2] - Snest[2];
//if (Math.Abs(D2x) > sup) D2x = 0;
D3x = Sprev[3] * Math.Cos(Math.PI / 4) - Snest[3] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D3x) > sup) D3x = 0;
D4x = -(Sprev[5] * Math.Cos(Math.PI / 4) - Snest[5] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D4x) > sup) D4x = 0;
D5x = -(Sprev[6] - Snest[6]);
//if (Math.Abs(D5x) > sup) D5x = 0;
D6x = -(Sprev[7] * Math.Cos(Math.PI / 4) - Snest[7] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D6x) > sup) D6x = 0;
gradient[0] = (D1x + D2x + D3x + D4x + D5x + D6x) / 6;
// computing Gy
D1y = Sprev[0] - Snest[0];
//if (Math.Abs(D1y) > sup) D1y = 0;
D2y = Sprev[1] * Math.Cos(Math.PI / 4) - Snest[1] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D2y) > sup) D2y = 0;
D3y = -(Sprev[3] * Math.Cos(Math.PI / 4) - Snest[3] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D3y) > sup) D3y = 0;
D4y = -(Sprev[4] - Snest[4]);
//if (Math.Abs(D4y) > sup) D4y = 0;
D5y = -(Sprev[5] * Math.Cos(Math.PI / 4) - Snest[5] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D5y) > sup) D5y = 0;
D6y = Sprev[7] * Math.Cos(Math.PI / 4) - Snest[7] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D6y) > sup) D6y = 0;
gradient[1] = (D1y + D2y + D3y + D4y + D5y + D6y) / 6;
return gradient;
}
// finds an estimate of the angular offset of the robot (unsigned)
// after a 90 turn
public static double getAngleOffset(LMMobileRobot thecart, int curOrientation, int prevOrientation)
{
double[] Snew = new double[8];
double[] Sprev = new double[8];
int i;
// storing sensor index permutations for both new and previous orientations
int[] prevPerm = getSensorPermutation(prevOrientation);
int[] newPerm = getSensorPermutation(curOrientation);
for (i = 0; i < 8; i++)
{
Snew[i] = (double)thecart.SonarArray[newPerm[i]];
Sprev[i] = (double)thecart.PrevSonarArray[prevPerm[i]];
}
// initially applying a Kalman Filter to the measurements
// a zero-input vector for the process
double[] U = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] B = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] W0 = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] V0 = new double[] { 2, 2, 2, 2, 2, 2, 2, 2 };
double[][] A = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] H = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] Q = new double[][] { new double[] { 5, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 5, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 5, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 5, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 5, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 5, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 5, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 5 }
};
double[][] R = new double[][] { new double[] { 2, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 2, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 2, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 2, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 2, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 2, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 2, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 2 }
};
KalmanFilter filter = new KalmanFilter(8, A, B, W0, H, V0, Q, R, Sprev);
// retrieving the measurement noise-free estimate by the Kalman filter,
// in order to use it to calculate the gradient
double[] Snest = filter.getNewEstimate(Snew, U);
// now computing the average cosine of the triangles
double anglesum = 0;
int count = 0;
for (i=0; i<8; i++)
if (Snest[i] < Snew[i])
{
count++;
anglesum += 180*Math.Acos((Snest[i]+11) / (Snew[i]+11))/Math.PI;
}
double average = (count == 0) ? 1 : anglesum / count;
return average;
}
// 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;
}
//Training FSM constructor
public CartTrainingFSM(LMMobileRobot cart)
{
Cart = cart;
state = stIdle;
}