Changeset 8865 for trunk/MagicSoft/Cosy/devdrv

Timestamp:

02/20/08 19:39:13 (17 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/devdrv

Files:

• macs.cc (modified) (11 diffs)
• macs.h (modified) (3 diffs)

trunk/MagicSoft/Cosy/devdrv/macs.cc

@@ -34 +34 @@
     : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
     fPos(0), fPdoPos(0), fPosActive(0), fRpmActive(0),
-    fStatusPdo3(0xff)
+    fStatusPdo3(0xff), fArmed(false)
 {
 //    fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
@@ -72 +72 @@
     case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2";
     case 0xa150: return "Drive controlled positioning with encoder 1";
-    case 0xa151: return "Drive controlled positioning with encoder 2";
+    case 0xa151: return "Drive controlled positioning with encoder 1, lagless";
     case 0xa208: return "Jog mode positive";
     case 0xa218: return "Jog mode negative";
@@ -91 +91 @@
     case 0xe264: return "Target position out of numerical range";
     case 0xe834: return "Emergency-Stop";
+    case 0xe842: return "Both end-switches activated";
     case 0xe843: return "Positive end-switch activated";
     case 0xe844: return "Negative end-switch activated";
@@ -150 +151 @@
     switch (idx)
     {
+    case 0x1000:
+        if (subidx==1)
+        {
+            gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
+            fArmed = val==1;
+        }
+        return;
+
     case 0x1003:
         // FIXME, see Init
@@ -382 +391 @@
             //lout << ddev(MLog::eGui);
             gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
+            fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
             //lout << edev(MLog::eGui);
             return;
@@ -388 +398 @@
             //lout << ddev(MLog::eGui);
             gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
+            fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
             //lout << edev(MLog::eGui);
             return;
@@ -428 +439 @@
     gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
     SendSDO(0x1000, 1, (LWORD_t)1);
+    WaitForSdo(0x1000, 1);
+}
+
+void Macs::Arm()
+{
+    StartNode();
+}
+
+void Macs::Disarm()
+{
+    gLog << inf2 << "- " << GetNodeName() << ": Stopping Node." << endl;
+    SendSDO(0x1000, 1, (LWORD_t)0);
     WaitForSdo(0x1000, 1);
 }
@@ -490 +513 @@
     //StartHostGuarding();
 
-    StartNode();
+    // REMOVE THIS AND LET CC START THE NODE
+    //StartNode();
+
+    gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
+    RequestSDO(0x1000, 1);
+    WaitForSdo(0x1000, 1);
 }
 /*
@@ -571 +599 @@
 void Macs::SetVelocity(LWORD_t vel)
 {
+    gLog << dbg << "- Setting velocity to: " << vel << endl;
     SendSDO(0x2002, vel);     // velocity
     WaitForSdo(0x2002, 0);
-    cout << "SET VEL: " << vel << endl;
 }
 
 void Macs::SetAcceleration(LWORD_t acc)
 {
+    gLog << dbg << "- Setting acceleration to: " << vel << endl;
     SendSDO(0x2003, 0, acc);  // acceleration
     WaitForSdo(0x2003, 0);
-    cout << "SET ACC: " << acc << endl;
 }
 
 void Macs::SetDeceleration(LWORD_t dec)
 {
-    SendSDO(0x2003, 1, dec);  // acceleration
+    gLog << dbg << "- Setting deceleration to: " << vel << endl;
+    SendSDO(0x2003, 1, dec);
     WaitForSdo(0x2003, 1);
-    cout << "SET DEC: " << dec << endl;
 }
 
@@ -607 +635 @@
 void Macs::StartRelPos(LWORDS_t pos)
 {
+    if (!fIsArmed)
+    {
+        gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
+        return;
+    }
+
+    gLog << dbg << GetNodeName() << ": Starting abolsute positioning to " << (LWORD_t)pos << " ticks." << endl;
     SendSDO(0x6004, 1, (LWORD_t)pos);
-    cout << "REL POS: " << (LWORD_t)pos << endl;
+    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
 }
 
 void Macs::StartAbsPos(LWORDS_t pos)
 {
+    if (!fIsArmed)
+    {
+        gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
+        return;
+    }
+    gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORD_t)pos << " ticks." << endl;
     SendSDO(0x6004, 0, (LWORD_t)pos);
-    cout << "ABS POS: " << (LWORD_t)pos << endl;
+    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
 }
 
@@ -681 +722 @@
     fInControl = data[3]&0x40; // motor uncontrolled
               // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
+
+    fArmed     = data[2]&kIsArmed==kIsArmed;
 
     fStatus = data[3];

trunk/MagicSoft/Cosy/devdrv/macs.h

@@ -34 +34 @@
 
     BYTE_t   fStatusPdo3;
+
+    bool fArmed;
 
     void Init();
@@ -73 +75 @@
     enum
     {
+        kIsArmed   = BIT(0),    // Macs will accept movement commands
         kNotMoving = BIT(0),    // motor not moving
         kPosActive = BIT(1),    // positioning active
@@ -121 +124 @@
     void StartNode();
 
+    bool IsArmed() const { return fArmed; }
+    void Arm();
+    void Disarm();
+
     int IsPositioning() const { return fPosActive; }
     BYTE_t GetStatus() const { return fStatus; }