etatBR robotGoToXY(ushort x, ushort y, sens s, bool boolDetection = false, int speed = 2)
{
etatBR retour;
if (boolDetection)
{
// on passe le sens "dir" au timer via la variable "state"
// analogue au timeout-callback pour les amoureux du js
//Timer t = new Timer(new TimerCallback(Detecter), s, 0, 1000);
obstacle = false; // paramètre pour savoir si il y'a bien un obstacle
var thDetection = new Thread(() =>
{
while (true)
{
Detecter(s);
//Thread.Sleep(20);
}
});
thDetection.Start();
retour = BaseRoulante.allerDect(x, y, s, speed);// x,y,s
thDetection.Suspend();
obstacle = false;
//t.Dispose();
}
else
{
retour = BaseRoulante.allerEn(x, y, s, speed);
}
Debug.Print("pos_visee " + x + " " + y);
return(retour);
}
public void Detecter(object o)
{
sens dir = (sens)o;
// si on avance, les ultrasons sont utiles
if (dir == sens.avancer)
{
if ((!IR.AVG.Read() || !IR.AVD.Read()))// infrarouge OK.. et c'est une condition et && obstacleUS
{
obstacle = true;
}
else
{
obstacle = false;
}
}
// si on recule, les ultrasons ne sont plus utiles
else
{
// on teste les capteurs IR arrières
if (!IR.ARG.Read() || !IR.ARD.Read())
{
obstacle = true;
}
else
{
obstacle = false;
}
}
}
//public void recalagePosX(int angle, int x,int speed,sens s)
//{
// bool reponse = true;
// m_posBR.alpha = angle;
// m_posBR.y = x;
// int r = 0;
// int distanceReelle=0;
// int AdistanceReelle=1;
// m_kangaroo.allerEn((int)(s)*100, speed, unite.mm);
// Thread.Sleep(150);
// while (reponse)
// {
// getDistanceParcourue(ref distanceReelle);
// if (distanceReelle!=AdistanceReelle) r=0;
// if (distanceReelle==AdistanceReelle) r++;
// if (r == 3)
// {
// m_kangaroo.start(mode.drive);
// //m_kangaroo.allerEn(0, speed, unite.mm);
// reponse = false;
// }
// Thread.Sleep(30);
// }
// Thread.Sleep(1000);
//}
public void recalagePosX(int angle, int x, int speed, sens s, int temps)
{
m_posBR.alpha = angle;
m_posBR.y = x;
m_kangaroo.allerEn((int)(s) * 150, speed, unite.mm);
Thread.Sleep(temps);
m_kangaroo.start(mode.drive);
}
public void recalagePosY(int angle, int y, int speed, sens s, int temps)
{
m_posBR.alpha = angle;
m_posBR.y = y;
// essaye de s'avancer 10 cm
m_kangaroo.allerEn((int)(s) * 150, speed, unite.mm);
Thread.Sleep(temps);
m_kangaroo.start(mode.drive);
}
public int setMovingSpeed(sens dir, int vitesse)
{
m_sens = dir;
byte len, error = 1;
int value = vitesse + (int)dir;
byte[] buf = { 0x20, (byte)(value), (byte)(value >> 8) };
sendCommand(Instruction.AX_WRITE_DATA, buf);
getReponse(out len, out error, null);
return((int)error);
}
public int stop()
{
m_sens = 0;
byte len, error = 1;
int value = 0;
byte[] buf = { 0x20, (byte)(value), (byte)(value >> 8) };
sendCommand(Instruction.AX_WRITE_DATA, buf);
getReponse(out len, out error, null);
return((int)error);
}
public void Detecter(object o)
{
sens dir = (sens)o;
// si on avance, les ultrasons sont utiles
if (dir == sens.avancer)
{
// on teste les capteurs IR avants puis le capteur laser en appui
/*
* double distance = 0d;
* bool obstacleUS = false;*/
// mesure une distance moyenne avec 5 mesures rapides
//Ultrason désactivé pour l'instant, ils prennent beaucoup trop de temps pour acquérir l'information.
/*
* m_ultrason.getDistance(1, ref distance);
* if (distance < 30 && distance != -1)
* obstacleUS = true;
*/
if ((!m_IR.AVG.Read() || !m_IR.AVD.Read()))// infrarouge OK.. et c'est une condition et && obstacleUS
{
//m_baseRoulante.stop();
obstacle = true;
}
else
{
obstacle = false;
}
}
// si on recule, les ultrasons ne sont plus utiles
else
{
// on teste les capteurs IR arrières
if (!m_IR.ARG.Read() || !m_IR.ARD.Read())
{
//Debug.Print("Détection obstacle après");
//m_baseRoulante.stop();
obstacle = true;
}
else
{
obstacle = false;
}
}
}
void recalageY(int angle, int y, sens s, int speed, int temps)
{
BaseRoulante.recalagePosY(angle, y, speed, s, temps);
}
void recalageX(int angle, int x, sens s, int speed, int temps)
{
BaseRoulante.recalagePosX(angle, x, speed, s, temps);
}
public etatBR allerDect(double x, double y, sens s, int speed = 1)
{
int posCodeur = 0;
int erreur = 0;
int distanceConsigne = 0, distanceReelle = 0, distanceReelle_tm1 = 0, distanceStop = 0, distanceSF = 0;
int alphaConsigne = 0, alphaReel = 0, alphaReel_tm1 = 0;
int delta = 0;
int ecart_t = 0, ecart_tm1 = 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;
int a = 1;
m_kangaroo.Tourner(alphaConsigne);
//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(500);
//relai.TurnOff();
Thread.Sleep(500);
m_kangaroo.init();
}
else
{
delta = System.Math.Abs(alphaConsigne - alphaReel);
if (delta < 3)
{
m_status = etatBR.arrive;
Thread.Sleep(200);
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);//unite.mm
do
{
if (GrandRobot.obstacle)
{
Debug.Print("entrée de la fonction check");
getDistanceParcourue(ref distanceStop);
//m_kangaroo.allerEn(0, 1, unite.mm);
m_kangaroo.start(mode.drive);
distanceSF += distanceStop;
while (GrandRobot.obstacle)
{
Thread.Sleep(100);
}
Debug.Print(distanceSF + "");
// m_kangaroo.allerEn(distanceConsigne + distanceSF, speed, unite.mm);//distanceConsigne - distanceSF
allerDect(x, y, s);
Debug.Print("sortie de la fonction check");
}
distanceReelle_tm1 = distanceReelle;
erreur = getDistanceParcourue(ref distanceReelle);
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 - (int)distanceSF);//(int) s*distanceSF
if (delta < 3)
{
m_status = etatBR.arrive;
Thread.Sleep(200);
erreur = getDistanceParcourue(ref distanceReelle);
}
if (erreur == 0xE3)
{
distanceReelle = distanceReelle_tm1;
m_status = etatBR.bloque;
//relai.TurnOn();
Thread.Sleep(500);
//relai.TurnOff();
Thread.Sleep(500);
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 - (int)distanceSF) * System.Math.Cos(m_posBR.alpha * System.Math.PI / 180)); //distanceReelle + (int)distanceSF
m_posBR.y = m_posBR.y + (int)((-distanceReelle - (int)distanceSF) * System.Math.Sin(m_posBR.alpha * System.Math.PI / 180)); //distanceReelle + (int)distanceSF
return(m_status);
}
public etatBR allerEn(double x, double y, sens s, int speed = 6)
{
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);
//attente d'être arrive ou bloque ou stoppe
do
{
alphaReel_tm1 = alphaReel;
// Thread.Sleep(1000);
erreur = getAngleTourne(ref alphaReel);
Debug.Print("" + erreur);
if (erreur == 0xE3)
{
alphaReel = alphaReel_tm1;
m_status = etatBR.bloque;
//relai.TurnOn();
Thread.Sleep(500);
//relai.TurnOff();
Thread.Sleep(500);
m_kangaroo.init();
}
else
{
delta = System.Math.Abs(alphaConsigne - alphaReel);
Debug.Print("delta " + delta);
if (delta < 3)
{
m_status = etatBR.arrive;
// pour l'inertie
Thread.Sleep(100);
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));
Debug.Print("distance à parcourir : " + distanceConsigne);
m_kangaroo.allerEn(distanceConsigne, speed, unite.mm);
//attente d'être arrive ou bloque ou stoppe
do
{
distanceReelle_tm1 = distanceReelle;
erreur = getDistanceParcourue(ref distanceReelle);
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 < 3)//5
{
m_status = etatBR.arrive;
Thread.Sleep(200);
erreur = getDistanceParcourue(ref distanceReelle);
}
if (erreur == 0xE3)
{
distanceReelle = distanceReelle_tm1;
m_status = etatBR.bloque;
//relai.TurnOn();
Thread.Sleep(500);
//relai.TurnOff();
Thread.Sleep(500);
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);
}
//CHANGE LA VITESSE DU MOTEUR
public bool setSpeedMotor(int numMoteur, sens direction)
{
bool retour = false;
int i = 0;
char[] cVitesse = new char[10];
byte[] commande = new byte[20];
byte[] reponse = new byte[20];
for (i = 0; i < 20; i++)
{
reponse[i] = 0;
}
if (m_serial.IsOpen)
{
commande[0] = (byte)'p';
commande[1] = (byte)'w';
commande[2] = (byte)'m';
commande[3] = (byte)' ';
commande[4] = (byte)numMoteur.ToString().ToCharArray()[0];
commande[5] = (byte)':';
// ON MET LA VITESSE(parametre direction: enum sens) DANS UN TABLEAU DE CHAR QUI VA ETRE MIT DANS LA COMMANDE ET VA ETRE ENVOYER
cVitesse = direction.ToString().ToCharArray();
for (i = 0; i < cVitesse.Length; i++)
commande[6 + i] = (byte)cVitesse[i];
commande[6 + i] = 0x0d;
m_serial.Write(commande, 0, 7 + cVitesse.Length);
if (getReponse(ref reponse) != 0)
retour = true;
}
return retour;
}
public bool tourner(sens direction)
{
return m_serializer.setSpeedMotor(m_numMoteur, direction);
}
