public Hokuyo(LidarID id, String portCom) : this(id)
{
_port = new SerialPort(portCom, 115200);
_port.Open();
IdentifyModel();
}
public HokuyoUsb(String portCom, LidarID id)
: base(id)
{
port = new SerialPort(portCom, 115200);
port.Open();
trameMesure = "MS0000" + nbPoints.ToString("0000") + "00001";
//trameDetails = "VV\n00P\n";
//port.WriteLine(trameDetails);
//String reponse = GetResultat();
//Console.WriteLine(reponse);
//if (reponse.Contains("UBG-04LX-F01")) //Hokuyo bleu
//{
// nbPoints = 725;
// angleMesurable = new Angle(240, AnglyeType.Degre);
// offsetPoints = 44;
//}
//else if (reponse.Contains("URG-04LX-UG01")) //Petit Hokuyo
//{
// nbPoints = 725;
// angleMesurable = new Angle(240, AnglyeType.Degre);
// offsetPoints = 44;
//}
//else if (reponse.Contains("BTM-75LX")) // Grand hokuyo
//{
// nbPoints = 1080;
// angleMesurable = new Angle(270, AnglyeType.Degre);
// offsetPoints = 0;
//}
}
public Hokuyo(LidarID lidar)
{
//trameDetails = "VV\n00P\n";
this.lidar = lidar;
semLock = new Semaphore(1, 1);
switch (lidar)
{
case LidarID.LidarSol: model = "URG-04LX-UG01"; break;
}
if (model.Contains("UBG-04LX-F01"))//Hokuyo bleu
{
nbPoints = 725;
angleMesurable = new Angle(240, AnglyeType.Degre);
offsetPoints = 44;
}
else if (model.Contains("URG-04LX-UG01")) //Petit hokuyo
{
nbPoints = 725;
angleMesurable = new Angle(240, AnglyeType.Degre);
offsetPoints = 44;
}
else if (model.Contains("BTM-75LX")) // Grand hokuyo
{
nbPoints = 1080;
angleMesurable = new Angle(270, AnglyeType.Degre);
offsetPoints = 0;
}
position = Robots.GrosRobot.Position;
Robots.GrosRobot.PositionChange += GrosRobot_PositionChange;
}
public override string ReadLidarMeasure(LidarID lidar, int timeout, out Position refPosition)
{
refPosition = new Position(_currentPosition);
// Pas de simulation de LIDAR
return("");
}
static public Frame DemandeMesureLidar(LidarID lidar)
{
byte[] tab = new byte[3];
tab[0] = (byte)Board.RecMove;
tab[1] = (byte)UdpFrameFunction.DemandeLidar;
tab[2] = (byte)lidar;
return(new Frame(tab));
}
public override String ReadLidarMeasure(LidarID lidar, int timeout, out Position refPosition)
{
_lastLidarMeasure = "";
_lockFrame[UdpFrameFunction.ReponseLidar] = new Semaphore(0, int.MaxValue);
Connections.ConnectionMove.SendMessage(UdpFrameFactory.DemandeMesureLidar(lidar));
_lockFrame[UdpFrameFunction.ReponseLidar].WaitOne(timeout);
refPosition = _lastLidarPosition;
return(_lastLidarMeasure);
}
private static Hokuyo CreateHokuyo(String portCom, LidarID id)
{
Hokuyo hok = null;
try
{
hok = new Hokuyo(id, portCom);
}
catch (Exception)
{
}
return(hok);
}
/// <summary>
/// Retourne le nom usuel d'un Lidar
/// </summary>
/// <param name="capteur">Lidar à nommer</param>
/// <returns>Nom du Lidar</returns>
public static String GetName(LidarID lidar)
{
switch (lidar)
{
case LidarID.Ground:
return("scan sol");
case LidarID.Avoid:
return("évitement");
default:
return(lidar.ToString());
}
}
public HokuyoRec(LidarID id) : base(id)
{
//_model = "UBG-04LX-F01";
//_pointsCount = 725;
_scanRange = 240;
//_pointsOffset = 44;
_distanceMinLimit = 50;
_distanceMaxLimit = 600;
_keepFrom = 175;
_keepTo = 575;
_invertRotation = false;
_resolution = 240 / 725.0;
_deltaX = 112;
_deltaY = 0;
}
public Hokuyo(LidarID id)
{
_id = id;
_lock = new Semaphore(1, 1);
_distanceMaxLimit = 3999; // En dessous de 4000 parce que le protocole choisi seuille le maximum à 4000 niveau matos
_position = new Position();
_lastMeasure = null;
_frameDetails = "VV\n";
_frameSpecif = "PP\n";
_frameLowSentitivity = "HS0\n";
_frameHighSentitivity = "HS1\n";
_invertRotation = false;
}
public static Hokuyo CreateHokuyo(String portCom, LidarID id)
{
bool forceUart = false;
bool forceUsb = false;
if (forceUart)
return new HokuyoUart(id);
else if (forceUsb)
return new HokuyoUsb(portCom, id);
Hokuyo hok;
try
{
hok = new HokuyoUsb(portCom, id);
return hok;
}
catch (Exception)
{
hok = new HokuyoUart(id);
return hok;
}
}
public abstract String ReadLidarMeasure(LidarID lidar, int timeout, out Position refPosition);
public static Trame DemandeMesureLidar(LidarID lidar)
{
byte[] tab = new byte[3];
tab[0] = (byte)Carte.RecIO;
tab[1] = (byte)FonctionIO.DemandeLidar;
tab[2] = (byte)lidar;
return new Trame(tab);
}
public override String GetMesureLidar(LidarID lidar, int timeout)
{
return "";
}
public override String GetMesureLidar(LidarID lidar, int timeout)
{
mesureLidar = "";
SemaphoresIO[FonctionIO.ReponseLidar] = new Semaphore(0, int.MaxValue);
Connexions.ConnexionIO.SendMessage(TrameFactory.DemandeMesureLidar(lidar));
SemaphoresIO[FonctionIO.ReponseLidar].WaitOne(timeout);
return mesureLidar;
}
public abstract String GetMesureLidar(LidarID lidar, int timeout);
public HokuyoUart(LidarID id)
: base(id)
{
}
