Changeset 1804 for trunk/MagicSoft/Cosy/devdrv

Timestamp:

03/02/03 17:08:13 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/devdrv

Files:

• macs.cc (modified) (5 diffs)
• macs.h (modified) (3 diffs)
• shaftencoder.cc (modified) (2 diffs)

trunk/MagicSoft/Cosy/devdrv/macs.cc

@@ -31 +31 @@
     : NodeDrv(nodeid, name, out), fMacId(2*nodeid+1),
     fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
-    fPosActive(0), fRpmActive(0)
+    fPosActive(0), fRpmActive(0), fStatusPdo3(0xff)
 {
 //    fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
@@ -57 +57 @@
 
     case 0x2002:
-        cout << GetNodeName() << ": Actual velocity: " << dec << val << endl;
+        cout << GetNodeName() << ": Current velocity: " << dec << val << endl;
         fVel = val;
         return;
@@ -273 +273 @@
     SetNoWait(TRUE);
 
-    StartGuarding(100, 2);
-    StartHostGuarding();
+    //    StartGuarding(500, 2);
+    //    StartHostGuarding();
 }
 
@@ -300 +300 @@
     SetPDO1On(FALSE);
 
-    lout << "- " << GetNodeName() << ": Motor off." << endl;
-    SendSDO(0x3000, string('o', 'f', 'f'));
-    WaitForSdo(0x3000);
+    /*
+     lout << "- " << GetNodeName() << ": Motor off." << endl;
+     SendSDO(0x3000, string('o', 'f', 'f'));
+     WaitForSdo(0x3000);
+     */
 
     /*
@@ -550 +552 @@
 }
 
+void Macs::HandlePDO3(BYTE_t *data, timeval_t *tv)
+{
+    // 3    5    7    9
+    // 1100 1010 1110 1001
+    if (fStatusPdo3 == data[3])
+        return;
+
+    lout << GetNodeName() << ": Status PDO3 = ";
+    const Bool_t ready = data[3]&0x01;
+    const Bool_t fuse  = data[3]&0x02;
+    const Bool_t emcy  = data[3]&0x04;
+    const Bool_t vltg  = data[3]&0x08;
+    const Bool_t mode  = data[3]&0x10;
+    const Bool_t rf    = data[3]&0x20;
+    const Bool_t brake = data[3]&0x40;
+    if (ready) lout << "DKC-Ready ";
+    if (fuse)  lout << "FuseOk ";
+    if (emcy)  lout << "EmcyOk ";
+    if (vltg)  lout << "OvervoltOk ";
+    if (mode)  lout << "SwitchToManualMode ";
+    if (rf)    lout << "RF ";
+    if (brake) lout << "BrakeOpen ";
+    lout << endl;
+
+    fStatusPdo3 = data[3];
+}
+
 // FIXME? Handling of fIsZombie?
 void Macs::HandleError()

trunk/MagicSoft/Cosy/devdrv/macs.h

@@ -28 +28 @@
     BYTE_t   fStatus;
 
-    LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
-    {
-        return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3);
-    }
+    BYTE_t   fStatusPdo3;
 
     void Init();
@@ -42 +39 @@
     void HandlePDO1(BYTE_t *data, timeval_t *tv);
     void HandlePDO2(BYTE_t *data, timeval_t *tv);
+    void HandlePDO3(BYTE_t *data, timeval_t *tv);
     //void HandleNodeguard(timeval_t *tv);
 
@@ -67 +65 @@
     Macs(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);
     virtual ~Macs();
+
+    static LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
+    {
+        return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3);
+    }
 
     void SendMsg(BYTE_t data[6]);

trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

@@ -252 +252 @@
 
     //
+    // Request Lifetimefactor for unknown reason to make guarding
+    // working in SE/Az... (FIXME)
+    //
+    //    lout << "- " << GetNodeName() << ": Requesting Lifetimefactor (Workaround, FIXME!) (SDO 0x100d)." << endl;
+    //    RequestSDO(0x100c);
+    //    WaitForSdo(0x100c);
+    //    RequestSDO(0x100d);
+    //    WaitForSdo(0x100d);
+
+    //
     // Set logic ticks/revolution = physical ticks/revolution => scale factor = 1
     //
@@ -291 +301 @@
     SendNMT(kNMT_START);
 
-    StartGuarding(175, 2);
+    /*
+     cout << "---1---" << endl;
+     MTimeout t(1000);
+     while (!t.HasTimedOut())
+      usleep(1);
+     cout << "---2---" << endl;
+     */
+
+    //    StartGuarding(175, 2); // 175
+    // StartGuarding(10*GetId(), 2); // 175
 }