public CanServo this[ServomoteurID servoGlobalId]
{
get
{
return(_servos[servoGlobalId]);
}
}
/// <summary>
/// Retourne le nom de la position en fonction du servomoteur (Ouvert, fermé, etc...)
/// </summary>
/// <param name="position">Position du servomoteur</param>
/// <param name="servo">Servomoteur</param>
/// <returns>Nom de la position</returns>
public static String GetName(int position, ServomoteurID servo)
{
PropertyInfo[] properties = typeof(Config).GetProperties();
foreach (PropertyInfo p in properties)
{
if (p.PropertyType.IsSubclassOf(typeof(PositionableServo)))
{
PositionableServo positionnableServo = (PositionableServo)(p.GetValue(Config.CurrentConfig, null));
if (positionnableServo.ID == servo)
{
PropertyInfo[] proprietesServo = positionnableServo.GetType().GetProperties();
foreach (PropertyInfo ps in proprietesServo)
{
if (ps.Name.StartsWith("Position") && ((int)(ps.GetValue(positionnableServo, null)) == position))
{
return(Config.PropertyNameToScreen(ps) + " (" + position + ")");
}
}
}
}
}
return(position.ToString());
}
/// <summary>
/// Retourne le nom usuel du servomoteur
/// </summary>
/// <param name="servo">Servomoteur recherché</param>
/// <returns>Nom usuel</returns>
public static String GetName(ServomoteurID servo)
{
switch (servo)
{
case ServomoteurID.Clamp1:
return("crochet gauche");
case ServomoteurID.Clamp2:
return("crochet milieu gauche");
case ServomoteurID.Clamp3:
return("crochet milieu");
case ServomoteurID.Clamp4:
return("crochet milieu droite");
case ServomoteurID.Clamp5:
return("crochet droite");
case ServomoteurID.FingerLeft:
return("doigt gauche");
case ServomoteurID.FingerRight:
return("doigt droite");
case ServomoteurID.FlagLeft:
return("drapeau gauche");
case ServomoteurID.FlagRight:
return("drapeau droite");
case ServomoteurID.GrabberLeft:
return("rabatteur gauche");
case ServomoteurID.GrabberRight:
return("rabatteur droite");
case ServomoteurID.Lifter:
return("monteur crochets");
case ServomoteurID.LockerLeft:
return("verrouilleur gauche");
case ServomoteurID.LockerRight:
return("verrouilleur droite");
case ServomoteurID.PushArmLeft:
return("bras sortie gauche");
case ServomoteurID.PushArmRight:
return("bras sortie droite");
case ServomoteurID.Tilter:
return("rotationneur crochets");
default:
return(servo.ToString());
}
}
private PositionableServo FindPositionnableFromServo(ServomoteurID servo)
{
PropertyInfo[] properties = Config.CurrentConfig.GetType().GetProperties();
List <PositionableServo> positionnables = properties.Where(o => typeof(PositionableServo).IsAssignableFrom(o.PropertyType)).Select(o => o.GetValue(Config.CurrentConfig, null)).Cast <PositionableServo>().ToList();
return(positionnables.Where(o => o.ID == servo).FirstOrDefault());
}
public CanServo(ServomoteurID id, iCanSpeakable communication)
{
_id = (ServomoteurID)((int)id % 100);
_communication = communication;
_lockResponse = new Semaphore(0, int.MaxValue);
_enableAutoCut = false;
_nextDisable = null;
}
public static Frame BuildDisableOutput(ServomoteurID id)
{
byte[] tab = new byte[10];
tab[0] = 0x00;
tab[1] = (byte)GlobalIdToCanBoard(id);
tab[2] = (byte)CanFrameFunction.DisableOutput;
tab[3] = GlobalIdToServoNo(id);
return(new Frame(tab));
}
public static Frame BuildGetAcceleration(ServomoteurID id)
{
byte[] tab = new byte[10];
tab[0] = 0x00;
tab[1] = (byte)GlobalIdToCanBoard(id);
tab[2] = (byte)CanFrameFunction.AccelerationAsk;
tab[3] = GlobalIdToServoNo(id);
return(new Frame(tab));
}
public static Frame BuildSetTorqueMax(ServomoteurID id, int torque)
{
byte[] tab = new byte[10];
tab[0] = 0x00;
tab[1] = (byte)GlobalIdToCanBoard(id);
tab[2] = (byte)CanFrameFunction.TorqueMaxSet;
tab[3] = GlobalIdToServoNo(id);
tab[4] = ByteDivide(torque, true);
tab[5] = ByteDivide(torque, false);
return(new Frame(tab));
}
public static Frame BuildSetPositionMin(ServomoteurID id, int position)
{
byte[] tab = new byte[10];
tab[0] = 0x00;
tab[1] = (byte)GlobalIdToCanBoard(id);
tab[2] = (byte)CanFrameFunction.PositionMinSet;
tab[3] = GlobalIdToServoNo(id);
tab[4] = ByteDivide(position, true);
tab[5] = ByteDivide(position, false);
return(new Frame(tab));
}
public void SetServo(ServomoteurID servo)
{
if (_servo is null || _servo.ID != servo)
{
numID.Value = (int)(servo);
_servo = AllDevices.CanServos[(ServomoteurID)numID.Value];
picConnection.Visible = false;
lblName.Text = "";
ReadValues();
_currentPositionable = FindPositionnableFromServo(servo);
SetPositions(_currentPositionable);
}
}
private void _communication_FrameReceived(Frame frame)
{
try
{
CanBoard idCan = CanFrameFactory.ExtractBoard(frame);
if (_canBoards.Contains(idCan))
{
ServomoteurID servoGlobalId = CanFrameFactory.ExtractServomoteurID(frame);
_servos[servoGlobalId].FrameReceived(frame);
}
}
catch (Exception e)
{
Console.WriteLine("ERREUR CAN : " + frame.ToString() + " - " + e.Message);
}
}
public static Frame BuildSetTrajectory(ServomoteurID id, int position, int speed, int accel)
{
byte[] tab = new byte[10];
tab[0] = 0x00;
tab[1] = (byte)GlobalIdToCanBoard(id);
tab[2] = (byte)CanFrameFunction.TrajectorySet;
tab[3] = GlobalIdToServoNo(id);
tab[4] = ByteDivide(position, true);
tab[5] = ByteDivide(position, false);
tab[6] = ByteDivide(speed, true);
tab[7] = ByteDivide(speed, false);
tab[8] = ByteDivide(accel, true);
tab[9] = ByteDivide(accel, false);
return(new Frame(tab));
}
/// <summary>
/// Retourne le nom usuel du servomoteur
/// </summary>
/// <param name="servo">Servomoteur recherché</param>
/// <returns>Nom usuel</returns>
public static String Nommer(ServomoteurID servo)
{
switch (servo)
{
case ServomoteurID.Tous:
return "tous les servomoteurs";
case ServomoteurID.BrasLunaireAvance:
return "bras lunaire linéaire";
case ServomoteurID.BrasLunaireMonte:
return "bras lunaire rotation";
case ServomoteurID.ServoLunaireSerrageDroit:
return "bras lunaire serrage droit";
case ServomoteurID.ServoLunaireSerrageGauche:
return "bras lunaire serrage gauche";
default:
if (servo.ToString().Contains("zzLibre"))
return "servo n°" + (int)servo;
else
return servo.ToString();
}
}
/// <summary>
/// Retourne le nom de la position en fonction du servomoteur (Ouvert, fermé, etc...)
/// </summary>
/// <param name="position">Position du servomoteur</param>
/// <param name="servo">Servomoteur</param>
/// <returns></returns>
public static String Nommer(int position, ServomoteurID servo)
{
PropertyInfo[] proprietes = typeof(Config).GetProperties();
foreach (PropertyInfo p in proprietes)
{
if (p.PropertyType.IsSubclassOf(typeof(PositionnableServo)))
{
PositionnableServo positionnableServo = (PositionnableServo)(p.GetValue(Config.CurrentConfig, null));
if (positionnableServo.ID == servo)
{
PropertyInfo[] proprietesServo = positionnableServo.GetType().GetProperties();
foreach (PropertyInfo ps in proprietesServo)
{
if (ps.Name.StartsWith("Position") && ((int)(ps.GetValue(positionnableServo, null)) == position))
return Config.PropertyNameToScreen(ps) + " (" + position + ")";
}
}
}
}
return position.ToString();
}
public static Trame ServoEnvoiId(ServomoteurID servo, char nouvelId, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiId;
tab[3] = (byte)servo;
tab[4] = (byte)nouvelId;
return new Trame(tab);
}
public static Trame ServoEnvoiConfigAlarmeShutdown(ServomoteurID servo, bool inputVoltage, bool angleLimit, bool overheating, bool range, bool checksum, bool overload, bool instruction, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[11];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiConfigAlarmeShutdown;
tab[3] = (byte)servo;
tab[4] = (byte)(inputVoltage ? 1 : 0);
tab[5] = (byte)(angleLimit ? 1 : 0);
tab[6] = (byte)(overheating ? 1 : 0);
tab[7] = (byte)(range ? 1 : 0);
tab[8] = (byte)(checksum ? 1 : 0);
tab[9] = (byte)(overload ? 1 : 0);
tab[10] = (byte)(instruction ? 1 : 0);
return new Trame(tab);
}
public static Trame ServoEnvoiConfigEcho(ServomoteurID servo, char val, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiConfigEcho;
tab[3] = (byte)servo;
tab[4] = (byte)val;
return new Trame(tab);
}
public static Trame ServoEnvoiBaudrate(ServomoteurID servo, ServoBaudrate baud, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiBaudrate;
tab[3] = (byte)servo;
tab[4] = (byte)baud;
return new Trame(tab);
}
public static Trame ServoEnvoiComplianceParams(ServomoteurID servo, byte CCWSlope, byte CCWMargin, byte CWMargin, byte CWSlope, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[8];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiComplianceParams;
tab[3] = (byte)servo;
tab[4] = CCWSlope;
tab[5] = CCWMargin;
tab[6] = CWMargin;
tab[7] = CWSlope;
return new Trame(tab);
}
private static CanBoard GlobalIdToCanBoard(ServomoteurID servoGlobalId)
{
return((CanBoard)((int)servoGlobalId / 4 + 1));
}
public static Trame ServoDemandeCoupleActuel(ServomoteurID servo, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[4];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.DemandeCoupleCourant;
tab[3] = (byte)servo;
return new Trame(tab);
}
public override void ServoVitesse(ServomoteurID servo, int vitesse)
{
}
private String Parse(ServomoteurID servo)
{
return(servo.ToString().Replace("Unused", ""));
}
public override void BougeServo(ServomoteurID servo, int position)
{
base.BougeServo(servo, position);
// Envoi à la pololu si c'est un servo géré par la carte
int idPololu = Servomoteur.idServoPololu(servo);
if (idPololu != -1)
PololuMiniUart.setTarget((byte)idPololu, (ushort)position);
// Sinon en UDP aux cartes elecs
else
{
if (this == Robots.GrosRobot)
{
Trame trame = TrameFactory.ServoEnvoiPositionCible(servo, position);
Connexions.ConnexionIO.SendMessage(trame);
}
}
Historique.AjouterAction(new ActionServo(this, position, servo));
}
public ActionServo(Robot r, int po, ServomoteurID pi)
{
robot = r;
position = po;
pince = pi;
}
public static Trame ServoEnvoiVitesseMax(ServomoteurID servo, int vitesse, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[6];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiVitesseMax;
tab[3] = (byte)servo;
tab[4] = ByteDivide(vitesse, true);
tab[5] = ByteDivide(vitesse, false);
return new Trame(tab);
}
public static Trame ServoEnvoiTemperatureMax(ServomoteurID servo, int temperature, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiTemperatureMax;
tab[3] = (byte)servo;
tab[4] = (byte)temperature;
return new Trame(tab);
}
private static byte GlobalIdToServoNo(ServomoteurID servoGlobalId)
{
return((byte)((int)servoGlobalId % 4));
}
public static Trame ServoEnvoiLed(ServomoteurID servo, bool allume, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiLed;
tab[3] = (byte)servo;
tab[4] = (byte)(allume ? 1 : 0);
return new Trame(tab);
}
public static Trame ServoDemandeConfigAlarmeShutdown(ServomoteurID servo, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[4];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.DemandeConfigAlarmeShutdown;
tab[3] = (byte)servo;
return new Trame(tab);
}
public static Trame ServoEnvoiPositionMinimum(ServomoteurID servo, int position, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[6];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiPositionMinimum;
tab[3] = (byte)servo;
tab[4] = ByteDivide(position, true);
tab[5] = ByteDivide(position, false);
return new Trame(tab);
}
public override void ServoVitesse(ServomoteurID servo, int vitesse)
{
Trame trame = TrameFactory.ServoEnvoiVitesseMax(servo, vitesse);
Connexions.ConnexionIO.SendMessage(trame);
}
public static Trame ServoEnvoiTensionMin(ServomoteurID servo, double tension, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[5];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.EnvoiTensionMin;
tab[3] = (byte)servo;
tab[4] = (byte)(tension * 10);
return new Trame(tab);
}
private void panelCan_ServoClick(ServomoteurID servoNo)
{
panelServoCan.SetServo(servoNo);
}
public static Trame ServoReset(ServomoteurID servo, Carte carte = Carte.RecIO)
{
byte[] tab = new byte[4];
tab[0] = (byte)carte;
tab[1] = (byte)FonctionIO.CommandeServo;
tab[2] = (byte)FonctionServo.Reset;
tab[3] = (byte)servo;
return new Trame(tab);
}
public virtual void BougeServo(ServomoteurID servo, int position)
{
if (ServoActive.ContainsKey(servo))
{
/*
* Todo
* if (servo == ServomoteurID.GRFruitsCoude && position != Config.CurrentConfig.PositionGRCoudeRange ||
servo == ServomoteurID.GRFruitsEpaule && position != Config.CurrentConfig.PositionGREpauleRange)
ServoActive[servo] = true;
else
ServoActive[servo] = false;*/
}
Thread.Sleep(10);
}
public ActionServo(Robot r, int po, ServomoteurID pi)
{
robot = r;
position = po;
pince = pi;
}
public override void BougeServo(ServomoteurID servo, int position)
{
base.BougeServo(servo, position);
Historique.AjouterAction(new ActionServo(this, position, servo));
}
public abstract void ServoVitesse(ServomoteurID servo, int vitesse);
public static short idServoPololu(ServomoteurID servo)
{
if (servosPololu.ContainsKey(servo))
return servosPololu[servo];
else
return -1;
}
