/// <summary>
/// Testing method for RelayX1 module (function never returns)
/// </summary>
static void TestRelay()
{
var relay = new RelayX1(FEZRaptor.Socket18.Pin5);
while (true)
{
relay.TurnOn();
Thread.Sleep(2000);
relay.TurnOff();
Thread.Sleep(2000);
}
}
public etatBR allerEn(double x, double y, sens s, int speed = 10, int speedAngle = 300)
{
int erreur = 0;
int distanceConsigne = 0, distanceReelle = 0, distanceReelle_tm1 = 0;
int alphaConsigne = 0, alphaReel = 0, alphaReel_tm1 = 0;
int delta = 0;
int dureeBlocage = 0;
alphaConsigne = (int)(System.Math.Atan2((y - m_posBR.y), (x - m_posBR.x)) * 180 / System.Math.PI) - m_posBR.alpha; //angle en degre
if (s == sens.reculer)
{
alphaConsigne = (int)(alphaConsigne + 180);
}
alphaConsigne = recallerAngle(alphaConsigne);
// m_status= tourner(alphaConsigne);
m_status = 0;
m_kangaroo.tourner(alphaConsigne, speedAngle);
//attente d'être arrive ou bloque ou stoppe
do
{
alphaReel_tm1 = alphaReel;
// Thread.Sleep(1000);
erreur = getAngleTourne(ref alphaReel);
if (erreur == 0xE3)
{
alphaReel = alphaReel_tm1;
m_status = etatBR.bloque;
relai.TurnOn();
Thread.Sleep(1000);
relai.TurnOff();
Thread.Sleep(1000);
m_kangaroo.init();
}
else
{
delta = System.Math.Abs(alphaConsigne - alphaReel);
if (delta < 2)
{
m_status = etatBR.arrive;
Thread.Sleep(300);
getAngleTourne(ref alphaReel);
}
}
} while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);
delta = 0;
m_status = 0;
distanceConsigne = (int)s * (int)System.Math.Sqrt(System.Math.Pow((x - m_posBR.x), 2) + System.Math.Pow((y - m_posBR.y), 2));
m_kangaroo.allerEn(distanceConsigne, speed, unite.mm);
int ij = 0;
//attente d'être arrive ou bloque ou stoppe
do
{
distanceReelle_tm1 = distanceReelle;
erreur = getDistanceParcourue(ref distanceReelle);
if (distanceReelle_tm1 == distanceReelle)
{
ij++;
}
else
{
ij = 0;
}
if (ij == 5)
{
break;
}
if (distanceReelle == 0)
{
dureeBlocage++;
if (dureeBlocage >= 10)
{
distanceReelle = distanceReelle_tm1;
m_status = etatBR.bloque;
m_kangaroo.start(mode.drive);
}
}
else
{
dureeBlocage = 0;
}
delta = System.Math.Abs(distanceConsigne - distanceReelle);
if (delta < 5)//5
{
m_status = etatBR.arrive;
Thread.Sleep(300);//500
erreur = getDistanceParcourue(ref distanceReelle);
}
if (erreur == 0xE3)
{
distanceReelle = distanceReelle_tm1;
m_status = etatBR.bloque;
relai.TurnOn();
Thread.Sleep(1000);
relai.TurnOff();
Thread.Sleep(1000);
m_kangaroo.init();
}
} while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);
m_posBR.alpha = m_posBR.alpha + alphaReel;
m_posBR.x = m_posBR.x + (int)(-distanceReelle * System.Math.Cos(m_posBR.alpha * System.Math.PI / 180));
m_posBR.y = m_posBR.y + (int)(-distanceReelle * System.Math.Sin(m_posBR.alpha * System.Math.PI / 180));
Debug.Print("position_x " + m_posBR.x + " position_y" + m_posBR.y);
/* m_kangaroo.powerdown(mode.drive);
* m_kangaroo.powerdown(mode.turn);
* m_kangaroo.init();*/
return(m_status);
}
public void TurnOff()
{
relayX1.TurnOff();
}
