AnimatLab  2
Test
DynamixelServo.cs
1 using System;
2 using System.Collections.Generic;
3 using System.Linq;
4 using System.Text;
5 
6 namespace RoboticsGUI
7 {
8  namespace RobotIOControls
9  {
10  public abstract class DynamixelServo : AnimatGUI.DataObjects.Robotics.MotorControlSystem
11  {
15  protected bool m_bResetToStartPos = false;
16 
17  protected bool m_bQueryMotorData = true;
18 
19  protected int m_iMinPosFP = 0;
20  protected int m_iMaxPosFP = 1023;
21  protected float m_fltMinAngle = -150;
22  protected float m_fltMaxAngle = 150;
23 
24  protected int m_iMinVelocityFP = 1;
25  protected int m_iMaxVelocityFP = 1023;
26  protected float m_fltRPMPerFPUnit = 0.111f;
27 
28  protected int m_iMinLoadFP = 0;
29  protected int m_iMaxLoadFP = 1023;
30 
31  protected int m_iCWComplianceMargin = 1;
32  protected int m_iCCWComplianceMargin = 1;
33 
34  protected int m_iCWComplianceSlope = 32;
35  protected int m_iCCWComplianceSlope = 32;
36 
37  protected int m_iMaxTorque = 1023;
38 
39  protected AnimatGUI.Framework.ScaledNumber m_snTranslationRange;
40 
41  public override string ModuleName { get { return "RoboticsAnimatSim"; } }
42  protected override System.Type GetLinkedPartDropDownTreeType() { return typeof(AnimatGUI.TypeHelpers.DropDownTreeEditorNoFirstSelect); }
43 
44  public virtual bool ResetToStartPos
45  {
46  get { return m_bResetToStartPos; }
47  set
48  {
49  SetSimData("ResetToStartPos", value.ToString(), true);
50  m_bResetToStartPos = value;
51  }
52  }
53 
54  public virtual bool QueryMotorData
55  {
56  get { return m_bQueryMotorData; }
57  set
58  {
59  SetSimData("QueryMotorData", value.ToString(), true);
60  m_bQueryMotorData = value;
61  }
62  }
63 
64  public virtual int MinPosFP
65  {
66  get { return m_iMinPosFP; }
67  set
68  {
69  if (value >= m_iMaxPosFP)
70  throw new System.Exception("The minimum fixed point position of the motor cannot be larger than the maximum value.");
71  SetSimData("MinPosFP", value.ToString(), true);
72  m_iMinPosFP = value;
73  }
74  }
75  public virtual int MaxPosFP
76  {
77  get { return m_iMaxPosFP; }
78  set
79  {
80  if (value <= m_iMinPosFP)
81  throw new System.Exception("The maximum fixed point position of the motor cannot be smaller than the minimum value.");
82  SetSimData("MaxPosFP", value.ToString(), true);
83  m_iMaxPosFP = value;
84  }
85  }
86 
87  public virtual float MinAngle
88  {
89  get { return m_fltMinAngle; }
90  set
91  {
92  if (value >= m_fltMaxAngle)
93  throw new System.Exception("The minimum angle of the motor cannot be larger than the maximum value.");
94  SetSimData("MinAngle", value.ToString(), true);
95  m_fltMinAngle = value;
96  }
97  }
98 
99  public virtual float MaxAngle
100  {
101  get { return m_fltMaxAngle; }
102  set
103  {
104  if (value <= m_fltMinAngle)
105  throw new System.Exception("The maximum angle of the motor cannot be smaller than the minimum value.");
106  SetSimData("MaxAngle", value.ToString(), true);
107  m_fltMaxAngle = value;
108  }
109  }
110 
111  public virtual int MinVelocityFP
112  {
113  get { return m_iMinVelocityFP; }
114  set
115  {
116  if (value >= m_iMaxVelocityFP)
117  throw new System.Exception("The minimum fixed point velocity of the motor cannot be larger than the maximum value.");
118  SetSimData("MinVelocityFP", value.ToString(), true);
119  m_iMinVelocityFP = value;
120  }
121  }
122  public virtual int MaxVelocityFP
123  {
124  get { return m_iMaxVelocityFP; }
125  set
126  {
127  if (value <= m_iMinVelocityFP)
128  throw new System.Exception("The maximum fixed point velocity of the motor cannot be smaller than the minimum value.");
129  SetSimData("MaxVelocityFP", value.ToString(), true);
130  m_iMaxVelocityFP = value;
131  }
132  }
133 
134  public virtual float RPMPerFPUnit
135  {
136  get { return m_fltRPMPerFPUnit; }
137  set
138  {
139  if (value <= 0)
140  throw new System.Exception("The RPM per FP unit of the motor cannot be less than or equal to zero.");
141  SetSimData("RPMPerFPUnit", value.ToString(), true);
142  m_fltRPMPerFPUnit = value;
143  }
144  }
145 
146  public virtual int MinLoadFP
147  {
148  get { return m_iMinLoadFP; }
149  set
150  {
151  if (value >= m_iMaxLoadFP)
152  throw new System.Exception("The minimum fixed point load of the motor cannot be larger than the maximum value.");
153  SetSimData("MinLoadFP", value.ToString(), true);
154  m_iMinLoadFP = value;
155  }
156  }
157 
158  public virtual int MaxLoadFP
159  {
160  get { return m_iMaxLoadFP; }
161  set
162  {
163  if (value <= m_iMinLoadFP)
164  throw new System.Exception("The maximum fixed point load of the motor cannot be smaller than the minimum value.");
165  SetSimData("MaxLoadFP", value.ToString(), true);
166  m_iMaxLoadFP = value;
167  }
168  }
169 
170  public virtual int CWComplianceMargin
171  {
172  get { return m_iCWComplianceMargin; }
173  set
174  {
175  if (value <= 0 || value > 255)
176  throw new System.Exception("The complaince margin must be between 0 and 255.");
177  SetSimData("CWComplianceMargin", value.ToString(), true);
178  m_iCWComplianceMargin = value;
179  }
180  }
181 
182  public virtual int CCWComplianceMargin
183  {
184  get { return m_iCCWComplianceMargin; }
185  set
186  {
187  if (value <= 0 || value > 255)
188  throw new System.Exception("The complaince margin must be between 0 and 255.");
189  SetSimData("CCWComplianceMargin", value.ToString(), true);
190  m_iCCWComplianceMargin = value;
191  }
192  }
193 
194  public virtual int CWComplianceSlope
195  {
196  get { return m_iCWComplianceSlope; }
197  set
198  {
199  if (value <= 0 || value > 255)
200  throw new System.Exception("The complaince slope must be between 0 and 255.");
201  SetSimData("CWComplianceSlope", value.ToString(), true);
202  m_iCWComplianceSlope = value;
203  }
204  }
205 
206  public virtual int CCWComplianceSlope
207  {
208  get { return m_iCCWComplianceSlope; }
209  set
210  {
211  if (value <= 0 || value > 255)
212  throw new System.Exception("The complaince slope must be between 0 and 255.");
213  SetSimData("CCWComplianceSlope", value.ToString(), true);
214  m_iCCWComplianceSlope = value;
215  }
216  }
217 
218  public virtual int MaxTorque
219  {
220  get { return m_iMaxTorque; }
221  set
222  {
223  if (value < 0 || value > 1023)
224  throw new System.Exception("The maximum torque of the motor must be between 0 and 1023.");
225  SetSimData("MaxTorque", value.ToString(), true);
226  m_iMaxTorque = value;
227  }
228  }
229 
230  public virtual AnimatGUI.Framework.ScaledNumber TranslationRange
231  {
232  get { return m_snTranslationRange; }
233  set
234  {
235  if (value.ActualValue <= 0)
236  throw new System.Exception("The translation range must be greater than zero.");
237 
238  SetSimData("TranslationRange", value.ActualValue.ToString(), true);
239  m_snTranslationRange.CopyData(ref value);
240  }
241  }
242 
243  public virtual bool IsHinge
244  {
245  get { return true; }
246  }
247 
248  public DynamixelServo(AnimatGUI.Framework.DataObject doParent)
249  : base(doParent)
250  {
251  m_strName = "Hinge Motor";
252 
253  m_thDataTypes.DataTypes.Add(new AnimatGUI.DataObjects.DataType("ReadParamTime", "Read Param Time", "Seconds", "s", 0, 1));
254  m_thDataTypes.DataTypes.Add(new AnimatGUI.DataObjects.DataType("IOPos", "IO Position", "", "", 0, 1024));
255  m_thDataTypes.DataTypes.Add(new AnimatGUI.DataObjects.DataType("IOVelocity", "IO Velocity", "", "", 0, 2048));
256 
257  m_snTranslationRange = new AnimatGUI.Framework.ScaledNumber(this, "TranslationRange", "meters", "m");
258  }
259 
260  public override void ClearIsDirty()
261  {
262  base.ClearIsDirty();
263 
264  if (m_snTranslationRange != null)
265  m_snTranslationRange.ClearIsDirty();
266 
267  }
268 
269  protected override void CloneInternal(AnimatGUI.Framework.DataObject doOriginal, bool bCutData, AnimatGUI.Framework.DataObject doRoot)
270  {
271  base.CloneInternal(doOriginal, bCutData, doRoot);
272 
273  DynamixelServo servo = (DynamixelServo)doOriginal;
274 
275  m_bResetToStartPos = servo.m_bResetToStartPos;
276  m_bQueryMotorData = servo.m_bQueryMotorData;
277 
278  m_iMinPosFP = servo.m_iMinPosFP;
279  m_iMaxPosFP = servo.m_iMaxPosFP;
280  m_fltMinAngle = servo.m_fltMinAngle;
281  m_fltMaxAngle = servo.m_fltMaxAngle;
282  m_iMinVelocityFP = servo.m_iMinVelocityFP;
283  m_iMaxVelocityFP = servo.m_iMaxVelocityFP;
284  m_fltRPMPerFPUnit = servo.m_fltRPMPerFPUnit;
285  m_iMinLoadFP = servo.m_iMinLoadFP;
286  m_iMaxLoadFP = servo.m_iMaxLoadFP;
287  m_snTranslationRange = (AnimatGUI.Framework.ScaledNumber)servo.m_snTranslationRange.Clone(this, bCutData, doRoot);
288  m_iCWComplianceMargin = servo.m_iCWComplianceMargin;
289  m_iCCWComplianceMargin = servo.m_iCCWComplianceMargin;
290  m_iCWComplianceSlope = servo.m_iCWComplianceSlope;
291  m_iCCWComplianceSlope = servo.m_iCCWComplianceSlope;
292  m_iMaxTorque = servo.m_iMaxTorque;
293  }
294 
295  public override void BuildProperties(ref AnimatGuiCtrls.Controls.PropertyTable propTable)
296  {
297  base.BuildProperties(ref propTable);
298 
299  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Reset To Start Pos", this.ResetToStartPos.GetType(), "ResetToStartPos",
300  "Properties", "If true then it will reset the joint to a known position at the start the simulation", this.ResetToStartPos));
301 
302  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Query Motor Data", this.QueryMotorData.GetType(), "QueryMotorData",
303  "Properties", "If this is false then no data is retrieved from the motor", this.QueryMotorData));
304 
305  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Position FP Min", this.MinPosFP.GetType(), "MinPosFP",
306  "Motor Properties", "This is the minimum fixed point position that the motor uses.", this.MinPosFP));
307 
308  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Position FP Max", this.MaxPosFP.GetType(), "MaxPosFP",
309  "Motor Properties", "This is the maximum fixed point position that the motor uses.", this.MaxPosFP));
310 
311  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Angle Min", this.MinAngle.GetType(), "MinAngle",
312  "Motor Properties", "This is the minimum angle the motor can attain.", this.MinAngle));
313 
314  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Angle Max", this.MaxAngle.GetType(), "MaxAngle",
315  "Motor Properties", "This is the maximum angle the motor can attain.", this.MaxAngle));
316 
317  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Velocity FP Min", this.MinVelocityFP.GetType(), "MinVelocityFP",
318  "Motor Properties", "This is the minimum fixed point velocity that the motor uses.", this.MinVelocityFP));
319 
320  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Velocity FP Max", this.MaxVelocityFP.GetType(), "MaxVelocityFP",
321  "Motor Properties", "This is the maximum fixed point velocity that the motor uses.", this.MaxVelocityFP));
322 
323  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("RPM Per FP Unit", this.RPMPerFPUnit.GetType(), "RPMPerFPUnit",
324  "Motor Properties", "This is the RPM per fixed velocity unit.", this.RPMPerFPUnit));
325 
326  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Load FP Min", this.MinLoadFP.GetType(), "MinLoadFP",
327  "Motor Properties", "This is the minimum fixed point load that the motor uses.", this.MinLoadFP));
328 
329  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Load FP Max", this.MaxLoadFP.GetType(), "MaxLoadFP",
330  "Motor Properties", "This is the maximum fixed point load that the motor uses.", this.MaxLoadFP));
331 
332  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("CW Compliance Margin", this.CWComplianceMargin.GetType(), "CWComplianceMargin",
333  "Motor Properties", "This is the clock-wise compliance margin used by this servo.", this.CWComplianceMargin));
334 
335  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("CCW Compliance Margin", this.CCWComplianceMargin.GetType(), "CCWComplianceMargin",
336  "Motor Properties", "This is the counter clock-wise compliance margin used by this servo.", this.CCWComplianceMargin));
337 
338  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("CW Compliance Slope", this.CWComplianceSlope.GetType(), "CWComplianceSlope",
339  "Motor Properties", "This is the clock-wise compliance slope used by this servo.", this.CWComplianceSlope));
340 
341  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("CCW Compliance Slope", this.CCWComplianceSlope.GetType(), "CCWComplianceSlope",
342  "Motor Properties", "This is the counter clock-wise compliance slope used by this servo.", this.CCWComplianceSlope));
343 
344  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Max Torque", this.MaxTorque.GetType(), "MaxTorque",
345  "Motor Properties", "This is the maximum torque setting used by this servo.", this.MaxTorque));
346 
347  if (!IsHinge)
348  {
349  AnimatGuiCtrls.Controls.PropertyBag pbNumberBag = m_snTranslationRange.Properties;
350  propTable.Properties.Add(new AnimatGuiCtrls.Controls.PropertySpec("Translation Range", pbNumberBag.GetType(), "TranslationRange",
351  "Motor Properties", "Sets the range of movement for a prismatic joint.", pbNumberBag,
352  "", typeof(AnimatGUI.Framework.ScaledNumber.ScaledNumericPropBagConverter)));
353  }
354  }
355 
356  public override void LoadData(ManagedAnimatInterfaces.IStdXml oXml)
357  {
358  base.LoadData(oXml);
359 
360  oXml.IntoElem();
361  m_bResetToStartPos = oXml.GetChildBool("ResetToStartPos", m_bResetToStartPos);
362  m_bQueryMotorData = oXml.GetChildBool("QueryMotorData", m_bQueryMotorData);
363  m_iMinPosFP = oXml.GetChildInt("MinPosFP", m_iMinPosFP);
364  m_iMaxPosFP = oXml.GetChildInt("MaxPosFP", m_iMaxPosFP);
365  m_fltMinAngle = oXml.GetChildFloat("MinAngle", m_fltMinAngle);
366  m_fltMaxAngle = oXml.GetChildFloat("MaxAngle", m_fltMaxAngle);
367  m_iMinVelocityFP = oXml.GetChildInt("MinVelocityFP", m_iMinVelocityFP);
368  m_iMaxVelocityFP = oXml.GetChildInt("MaxVelocityFP", m_iMaxVelocityFP);
369 
370  m_iCWComplianceMargin = oXml.GetChildInt("CWComplianceMargin", m_iCWComplianceMargin);
371  m_iCCWComplianceMargin = oXml.GetChildInt("CCWComplianceMargin", m_iCCWComplianceMargin);
372 
373  m_iCWComplianceSlope = oXml.GetChildInt("CWComplianceSlope", m_iCWComplianceSlope);
374  m_iCCWComplianceSlope = oXml.GetChildInt("CCWComplianceSlope", m_iCCWComplianceSlope);
375 
376  m_iMaxTorque = oXml.GetChildInt("MaxTorque", m_iMaxTorque);
377 
378  if (oXml.FindChildElement("MaxRotMin", false))
379  m_fltRPMPerFPUnit = oXml.GetChildFloat("MaxRotMin", m_fltRPMPerFPUnit);
380  else
381  m_fltRPMPerFPUnit = oXml.GetChildFloat("RPMPerFPUnit", m_fltRPMPerFPUnit);
382 
383  m_iMinLoadFP = oXml.GetChildInt("MinLoadFP", m_iMinLoadFP);
384  m_iMaxLoadFP = oXml.GetChildInt("MaxLoadFP", m_iMaxLoadFP);
385 
386  if (!IsHinge && oXml.FindChildElement("TranslationRange", false))
387  m_snTranslationRange.LoadData(oXml, "TranslationRange");
388 
389  oXml.OutOfElem();
390  }
391 
392  public override void SaveData(ManagedAnimatInterfaces.IStdXml oXml)
393  {
394  base.SaveData(oXml);
395 
396  oXml.IntoElem();
397  oXml.AddChildElement("ResetToStartPos", m_bResetToStartPos);
398  oXml.AddChildElement("QueryMotorData", m_bQueryMotorData);
399  oXml.AddChildElement("MinPosFP", m_iMinPosFP);
400  oXml.AddChildElement("MaxPosFP", m_iMaxPosFP);
401  oXml.AddChildElement("MinAngle", m_fltMinAngle);
402  oXml.AddChildElement("MaxAngle", m_fltMaxAngle);
403  oXml.AddChildElement("MinVelocityFP", m_iMinVelocityFP);
404  oXml.AddChildElement("MaxVelocityFP", m_iMaxVelocityFP);
405  oXml.AddChildElement("RPMPerFPUnit", m_fltRPMPerFPUnit);
406  oXml.AddChildElement("MinLoadFP", m_iMinLoadFP);
407  oXml.AddChildElement("MaxLoadFP", m_iMaxLoadFP);
408 
409  oXml.AddChildElement("CWComplianceMargin", m_iCWComplianceMargin);
410  oXml.AddChildElement("CCWComplianceMargin", m_iCCWComplianceMargin);
411 
412  oXml.AddChildElement("CWComplianceSlope", m_iCWComplianceSlope);
413  oXml.AddChildElement("CCWComplianceSlope", m_iCCWComplianceSlope);
414 
415  oXml.AddChildElement("MaxTorque", m_iMaxTorque);
416 
417  if (!IsHinge)
418  m_snTranslationRange.SaveData(oXml, "TranslationRange");
419 
420  oXml.OutOfElem();
421  }
422 
423  public override void SaveSimulationXml(ManagedAnimatInterfaces.IStdXml oXml, ref AnimatGUI.Framework.DataObject nmParentControl, string strName = "")
424  {
425  base.SaveSimulationXml(oXml, ref nmParentControl, strName);
426 
427  oXml.IntoElem();
428  oXml.AddChildElement("ResetToStartPos", m_bResetToStartPos);
429  oXml.AddChildElement("QueryMotorData", m_bQueryMotorData);
430  oXml.AddChildElement("MinPosFP", m_iMinPosFP);
431  oXml.AddChildElement("MaxPosFP", m_iMaxPosFP);
432  oXml.AddChildElement("MinAngle", m_fltMinAngle);
433  oXml.AddChildElement("MaxAngle", m_fltMaxAngle);
434  oXml.AddChildElement("MinVelocityFP", m_iMinVelocityFP);
435  oXml.AddChildElement("MaxVelocityFP", m_iMaxVelocityFP);
436  oXml.AddChildElement("RPMPerFPUnit", m_fltRPMPerFPUnit);
437  oXml.AddChildElement("MinLoadFP", m_iMinLoadFP);
438  oXml.AddChildElement("MaxLoadFP", m_iMaxLoadFP);
439  oXml.AddChildElement("IsHinge", IsHinge);
440 
441  oXml.AddChildElement("CWComplianceMargin", m_iCWComplianceMargin);
442  oXml.AddChildElement("CCWComplianceMargin", m_iCCWComplianceMargin);
443 
444  oXml.AddChildElement("CWComplianceSlope", m_iCWComplianceSlope);
445  oXml.AddChildElement("CCWComplianceSlope", m_iCCWComplianceSlope);
446 
447  oXml.AddChildElement("MaxTorque", m_iMaxTorque);
448 
449  if (!IsHinge)
450  m_snTranslationRange.SaveSimulationXml(oXml, ref nmParentControl, "TranslationRange");
451 
452  oXml.OutOfElem();
453  }
454  }
455  }
456 }
RoboticsGUI.RobotIOControls.DynamixelServo.m_bResetToStartPos
bool m_bResetToStartPos
If true then when the simulation starts it will always reset the position of the servo to 0 to begin ...
Definition: DynamixelServo.cs:15