public Tracks Movement(Tracks ip, Voltages V, double t)
{ // ip - initial position of tracks, V - Velocities, t - time
double k = 0.001;
double a = 0.0;
double b = 0.0;
double l = 0.0;
double lx = 0.0;
double ly = 0.0;
double rx = 0.0;
double ry = 0.0;
if (Math.Abs(ip.lx - ip.rx) < k)
{
a = Math.PI / 2.0;
b = Math.PI - a;
l = Math.Abs((ip.ly - ip.ry) / 2.0);
}
else
{
a = Math.Atan((ip.ly - ip.ry) / (ip.lx - ip.rx));
if (a <= 0.0)
{
a = Math.PI + a;
}
b = Math.PI - a;
l = Math.Abs((ip.lx - ip.rx) / (2.0 * Math.Cos(b)));
}
if (Math.Abs(V.lV - V.rV) < k)
{
double S = 0.0;
double c = 0.0;
S = ((V.lV + V.rV) / 2.0) * t;
if (ip.lx > ip.rx)
{
S = -S;
}
else if ((Math.Abs(ip.lx - ip.rx) < k) && (ip.ly < ip.ry))
{
S = -S;
}
if (a >= (Math.PI / 2.0))
{
c = a - Math.PI / 2.0;
}
else
{
c = Math.PI / 2.0 + a;
}
lx = ip.lx + S * Math.Cos(c);
ly = ip.ly + S * Math.Sin(c);
rx = ip.rx + S * Math.Cos(c);
ry = ip.ry + S * Math.Sin(c);
}
else
{
double R = 0.0;
double c = 0.0;
double Rx = 0.0;
double Ry = 0.0;
double lr = 0.0;
double rr = 0.0;
double ld = 0.0;
double rd = 0.0;
R = ((V.lV + V.rV) / (V.lV - V.rV)) * l;
if (Math.Abs(R - l) < k)
{
c = (V.lV / (R + l)) * t;
}
else
{
c = (V.rV / (R - l)) * t;
}
rx = ip.rx - ip.lx;
ry = ip.ry - ip.ly;
Rx = GetX(GetPolarR(rx, ry) + (R - l), GetPolarF(rx, ry)) + ip.lx;
Ry = GetY(GetPolarR(rx, ry) + (R - l), GetPolarF(rx, ry)) + ip.ly;
lx = ip.lx - Rx;
ly = ip.ly - Ry;
rx = ip.rx - Rx;
ry = ip.ry - Ry;
lr = GetPolarR(lx, ly);
rr = GetPolarR(rx, ry);
ld = GetPolarF(lx, ly) - c;
while (ld < 0)
{
ld = 2.0 * Math.PI + ld;
}
while (ld > 2.0 * Math.PI)
{
ld = ld - 2.0 * Math.PI;
}
rd = GetPolarF(rx, ry) - c;
while (rd < 0)
{
rd = 2.0 * Math.PI + rd;
}
while (rd > 2.0 * Math.PI)
{
rd = rd - 2.0 * Math.PI;
}
lx = GetX(lr, ld) + Rx;
ly = GetY(lr, ld) + Ry;
rx = GetX(rr, rd) + Rx;
ry = GetY(rr, rd) + Ry;
}
return(new Tracks(lx, ly, rx, ry));
}
// Sugeno-Type Fuzzy Inference
public Voltages LogicMethod(double ld, double rd)
{ // ld, rd - distances from sensors
//Fuzzification
List <Voltages> V = new List <Voltages>();
List <double> m = new List <double>();
// Rules
if ((high(ld) > 0) && (high(rd) > 0))
{
Voltages U1 = new Voltages(Umax, Umax);
V.Add(U1);
if (high(ld) < high(rd))
{
m.Add(high(ld));
}
else
{
m.Add(high(rd));
}
}
if ((medium(ld) > 0) && (high(rd) > 0))
{
Voltages U2 = new Voltages(Umax, k * Umax);
V.Add(U2);
if (medium(ld) < high(rd))
{
m.Add(medium(ld));
}
else
{
m.Add(high(rd));
}
}
if ((high(ld) > 0) && (medium(rd) > 0))
{
Voltages U3 = new Voltages(k * Umax, Umax);
V.Add(U3);
if (high(ld) < medium(rd))
{
m.Add(high(ld));
}
else
{
m.Add(medium(rd));
}
}
if ((medium(ld) > 0) && (medium(rd) > 0))
{
Voltages U4;
if (ld <= rd)
{
U4 = new Voltages(Umax, 0);
}
else
{
U4 = new Voltages(0, Umax);
}
V.Add(U4);
if (medium(ld) < medium(rd))
{
m.Add(medium(ld));
}
else
{
m.Add(medium(rd));
}
}
if ((low(ld) > 0) && (high(rd) > 0))
{
Voltages U5 = new Voltages(Umax, 0 - k * Umax);
V.Add(U5);
if (low(ld) < high(rd))
{
m.Add(low(ld));
}
else
{
m.Add(high(rd));
}
}
if ((high(ld) > 0) && (low(rd) > 0))
{
Voltages U6 = new Voltages(0 - k * Umax, Umax);
V.Add(U6);
if (high(ld) < low(rd))
{
m.Add(high(ld));
}
else
{
m.Add(low(rd));
}
}
if ((low(ld) > 0) && (medium(rd) > 0))
{
Voltages U7 = new Voltages(Umax, 0 - Umax);
V.Add(U7);
if (low(ld) < medium(rd))
{
m.Add(low(ld));
}
else
{
m.Add(medium(rd));
}
}
if ((medium(ld) > 0) && (low(rd) > 0))
{
Voltages U8 = new Voltages(0 - k * Umax, Umax);
V.Add(U8);
if (medium(ld) < low(rd))
{
m.Add(medium(ld));
}
else
{
m.Add(low(rd));
}
}
if ((low(ld) > 0) && (low(rd) > 0))
{
Voltages U9;
//if (ld <= rd)
U9 = new Voltages(Umax, 0 - Umax);
//else
//U9 = new Voltages(0 - Umax, Umax);
V.Add(U9);
if (low(ld) < low(rd))
{
m.Add(low(ld));
}
else
{
m.Add(low(rd));
}
}
// Defuzzification
double lV = 0.0;
double rV = 0.0;
double M = 0.0;
for (int i = 0; i < V.Count; i++)
{
lV = lV + m[i] * V[i].lV;
rV = rV + m[i] * V[i].rV;
M = M + m[i];
}
lV = lV / M;
rV = rV / M;
return(new Voltages(lV, rV));
}
