public double[] CalculaValorXSecanteCircuferencia(Reta r1)
{
double x1, x2;
x1 = x2 = 0;
if (r1.RetaVertical)
{
x1 = x2 = r1.P1.X;
}
else
{
double a = (1 + Math.Pow(r1.M, 2));
double b = (2 * r1.M * r1.B) - (2 * r1.M * this._c.Y) - (2 * this._c.X);
double c = (Math.Pow(this._c.X, 2)) + (Math.Pow(r1.B, 2)) + (Math.Pow(this._c.Y, 2)) - (2 * r1.B * this._c.Y) - (Math.Pow(this._r, 2));
double delta = Math.Pow(b, 2) - (4 * a * c);
if (delta > 0)
{
if (a != 0)
{
x1 = (((-1 * b) + (Math.Sqrt(delta))) / (2 * a));
x2 = (((-1 * b) - (Math.Sqrt(delta))) / (2 * a));
}
}
}
double[] arrRetorno = new double[2];
arrRetorno[0] = x1;
arrRetorno[1] = x2;
return(arrRetorno);
}
public double[] CalculaValorXSecanteCircuferencia(Reta r1)
{
double x1, x2;
x1 = x2 = 0;
if (r1.RetaVertical)
{
x1 = x2 = r1.P1.X;
}
else
{
double a = (1 + Math.Pow(r1.M, 2));
double b = (2 * r1.M * r1.B) - (2 * r1.M * this._c.Y) - (2 * this._c.X);
double c = (Math.Pow(this._c.X, 2)) + (Math.Pow(r1.B, 2)) + (Math.Pow(this._c.Y, 2)) - (2 * r1.B * this._c.Y) - (Math.Pow(this._r, 2));
double delta = Math.Pow(b, 2) - (4 * a * c);
if (delta > 0)
{
if (a != 0)
{
x1 = (((-1 * b) + (Math.Sqrt(delta))) / (2 * a));
x2 = (((-1 * b) - (Math.Sqrt(delta))) / (2 * a));
}
}
}
double[] arrRetorno = new double[2];
arrRetorno[0] = x1;
arrRetorno[1] = x2;
return arrRetorno;
}
// Getting sensors measures
private void GetMeasures()
{
bool blVirou = false;
// Getting AGV's position
pbTerrain.Image = CopyImage(OriginalMap);
Bitmap b = pbTerrain.Image as Bitmap;
Point pPos = new Point(pbRobot.Left - pbTerrain.Left + 5, pbRobot.Top - pbTerrain.Top + 5);
// AGV on the wall
if ((b.GetPixel(pPos.X, pPos.Y).R == 0) && (b.GetPixel(pPos.X, pPos.Y).G == 0) && (b.GetPixel(pPos.X, pPos.Y).B == 0))
{
if (btnRun.Text != RunLabel)
{
btnRun_Click(btnRun, null);
}
if (btnRun.Enabled)
{
string Msg = "O veiculo atingiu uma barreira!";
MessageBox.Show(Msg, "Error!");
btnRun.Enabled = false;
}
}
double radAngle = ((Angle + 90) * Math.PI) / 180;
#region Nova logica retas
//Point pFront = GetObstacle(new Point(pPos.X, pPos.Y), b, -1, 0);
//Point pFront = GetObstacle(new Point(pPos.X, pPos.Y), b, 1, 5);
Point pFront = GetObstacle(new Point(pPos.X, pPos.Y), b, 10, 0);
Reta rPrincipal = new Reta(pPos, pFront);
double mperpendicularprincipal = rPrincipal.CalculaMPerpendicularAReta();
Reta rperpendicularprincipal = new Reta(mperpendicularprincipal, pPos, null);
Circulo crobo = new Circulo(pPos, 5);
double[] xsecantes = crobo.CalculaValorXSecanteCircuferencia(rperpendicularprincipal);
double[] ysecantes = new double[2];
if (xsecantes.Length == 2)
{
if (rperpendicularprincipal.RetaVertical)
{
//Caso a reta perpendicular a principal seja uma reta vertical, utilizar o circulo para determinar a posião dos Y, pois, a equação da reta estara definida como "x = k"
ysecantes = crobo.CalculaValorY(xsecantes[0]);
}
else
{
ysecantes[0] = rperpendicularprincipal.CalculaValorY(xsecantes[0]);
ysecantes[1] = rperpendicularprincipal.CalculaValorY(xsecantes[1]);
}
}
Point pPrincipalParalela1 = new Point((int)xsecantes[0], (int)ysecantes[0]);
Point pPrincipalParalela2 = new Point((int)xsecantes[1], (int)ysecantes[1]);
Reta rPrincipalParalela1 = new Reta(rPrincipal.M, pPrincipalParalela1, null);
Reta rPrincipalParalela2 = new Reta(rPrincipal.M, pPrincipalParalela2, null);
Point pFrontObstacle1 = GetObstacle(new Point(pPrincipalParalela1.X, pPrincipalParalela1.Y), b, -1, 0);
Point pFrontObstacle2 = GetObstacle(new Point(pPrincipalParalela2.X, pPrincipalParalela2.Y), b, -1, 0);
Point pLeft45Obstacle = GetObstacle(new Point(pPos.X, pPos.Y), b, -1, -45);
Point pRight45Obstacle = GetObstacle(new Point(pPos.X, pPos.Y), b, -1, 45);
Point pLeftObstacle = GetObstacle(new Point(pPos.X, pPos.Y), b, 1, 90);
Point pRightObstacle = GetObstacle(new Point(pPos.X, pPos.Y), b, 1, -90);
#endregion
// Showing beams
Graphics g = Graphics.FromImage(b);
if (cbLasers.Checked)
{
//Adicionando retas
//Frontais
g.DrawLine(new Pen(Color.Green, 1), pFrontObstacle1, pPrincipalParalela1);
g.DrawLine(new Pen(Color.Green, 1), pFrontObstacle2, pPrincipalParalela2);
//Laterais
g.DrawLine(new Pen(Color.Red, 1), pLeftObstacle, pPos);
g.DrawLine(new Pen(Color.Red, 1), pRightObstacle, pPos);
g.DrawLine(new Pen(Color.Red, 1), pLeft45Obstacle, pPos);
g.DrawLine(new Pen(Color.Red, 1), pRight45Obstacle, pPos);
}
// Drawing AGV
if (btnRun.Text != RunLabel)
{
g.FillEllipse(new SolidBrush(Color.Navy), pPos.X - 5, pPos.Y - 5, 10, 10);
}
g.DrawImage(b, 0, 0);
g.Dispose();
pbTerrain.Refresh();
double VlLimiarDistancia = Convert.ToDouble(txtLimiarDistancia.Text);
// Updating distances texts
//Esquerda
if (!chkFalhaSensorFrontal0.Checked)
{
txtFront0.Text = GetDistance(pPos, pLeftObstacle).ToString();
sensors[0] = !(Convert.ToDouble(txtFront0.Text) > VlLimiarDistancia);
pbLimiar0.Visible = sensors[0];
}
//Esquerda 45
if (!chkFalhaSensorFrontal1.Checked)
{
txtFront1.Text = GetDistance(pPos, pLeft45Obstacle).ToString();
sensors[1] = !(Convert.ToDouble(txtFront1.Text) > VlLimiarDistancia);
pbLimiar1.Visible = sensors[1];
}
//Frente
if (!chkFalhaSensorFrontal2.Checked)
{
txtFront2.Text = GetDistance(pPrincipalParalela1, pFrontObstacle1).ToString();
sensors[2] = !(Convert.ToDouble(txtFront2.Text) > VlLimiarDistancia);
pbLimiar2.Visible = sensors[2];
}
if (!chkFalhaSensorFrontal3.Checked)
{
txtFront3.Text = GetDistance(pPrincipalParalela2, pFrontObstacle2).ToString();
sensors[3] = !(Convert.ToDouble(txtFront3.Text) > VlLimiarDistancia);
pbLimiar3.Visible = sensors[3];
}
//Direita 45
if (!chkFalhaSensorFrontal4.Checked)
{
txtFront4.Text = GetDistance(pPos, pRight45Obstacle).ToString();
sensors[4] = !(Convert.ToDouble(txtFront4.Text) > VlLimiarDistancia);
pbLimiar4.Visible = sensors[4];
}
//Direita
if (!chkFalhaSensorFrontal5.Checked)
{
txtFront5.Text = GetDistance(pPos, pRightObstacle).ToString();
sensors[5] = !(Convert.ToDouble(txtFront5.Text) > VlLimiarDistancia);
pbLimiar5.Visible = sensors[5];
}
//Trazeiro
//txtFront6.Text = GetDistance(pPrincipalParalela1, pBehindObstacle1).ToString();
//sensors[6] = !(Convert.ToDouble(txtFront6.Text) > VlLimiarDistancia);
//pbLimiar6.Visible = sensors[6];
//txtFront7.Text = GetDistance(pPrincipalParalela2, pBehindObstacle2).ToString();
//sensors[7] = !(Convert.ToDouble(txtFront7.Text) > VlLimiarDistancia);
//pbLimiar7.Visible = sensors[7];
//A velocidade é determinada em tempo real de acordo com a distancia dos obstaculos
//Se um dos sensores atingiu o valor da limiar diminui a velocidade
//Utilizar dois sensores proximos para determinar qundo esta p´rximo de um obstaculo
//if (!((sensors[0] && sensors[1]) || (sensors[2] && sensors[3]) || (sensors[4] && sensors[5]) || (sensors[6] && sensors[7])))
if (!((sensors[0] && sensors[1]) || (sensors[1] && sensors[2]) || (sensors[2] && sensors[3]) || (sensors[3] && sensors[4]) || (sensors[4] && sensors[5])))
{
//txtSpeed.Text = "12,0";
if ((Speed + 3) < Convert.ToInt32(txtVelFim.Text))
Speed += 3;
if (_virou)
Speed = 0;
}
else
{
//txtSpeed.Text = "5,0";
//Speed = 5;
if ((Speed - 3) > 3)
Speed -= 3;
else if (!_virou)
Speed = 3;
if (_virou)
Speed = 0;
}
txtSpeed.Text = Speed.ToString();
Angle = Angle % 360;
txtAngle.Text = Angle.ToString();
}