Changeset 9439 for trunk/MagicSoft/Cosy/devdrv

Timestamp:

05/09/09 13:48:12 (15 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/devdrv

Files:

• dkc.cc (modified) (15 diffs)
• dkc.h (modified) (3 diffs)

trunk/MagicSoft/Cosy/devdrv/dkc.cc

@@ -22 +22 @@
     fStatus(0), fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
     fReport(NULL),fLabel(NULL),fUpdPos(0)
-{
-}
-
-Dkc::~Dkc()
 {
 }
@@ -165 +161 @@
     {
     case 0x1000:
-        if (subidx==1)
-        {
-            gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
-            fArmed = val==1;
-        }
+        gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
+        fArmed = val==1;
         return;
 
     case 0x1003:
         // FIXME, see Init
-        if (subidx!=2)
-            return;
         gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl;
         CheckErrorDKC(val);
@@ -196 +187 @@
 
     case 0x6004:
-        if (subidx==0)
-        {
-            fPdoPos1 = (LWORDS_t)val;
-            fPdoTime1.Set(tv);
-            fHasChangedPos1 = true;
-        }
-        if (subidx==1)
-        {
-            fPdoPos2 = (LWORDS_t)val;
-            fPdoTime2.Set(tv);
-            fHasChangedPos2 = true;
-        }
+        fPdoPos1 = (LWORDS_t)val;
+        fPdoTime1.Set(tv);
+        fHasChangedPos1 = true;
+        return;
+
+    case 0x6005:
+        fPdoPos2 = (LWORDS_t)val;
+        fPdoTime2.Set(tv);
+        fHasChangedPos2 = true;
         return;
 
@@ -246 +234 @@
     {
     case 0x1000:
-        switch (subidx)
-        {
-        case 1:
-            //lout << ddev(MLog::eGui);
-            gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
-            //lout << edev(MLog::eGui);
-            return;
-        }
-        break;
+        gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
+        return;
+
+    case 0x1001:
+        gLog << inf2 << "- " << GetNodeName() << ": PDOs requested." << endl;
+        return;
 
     case 0x2002:
-        //lout << ddev(MLog::eGui);
         gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
-        //lout << edev(MLog::eGui);
         return;
 
     case 0x2003:
-        //lout << ddev(MLog::eGui);
         gLog << inf2 << "- " << GetNodeName() << ": Acceleration set." << endl;
-        //lout << edev(MLog::eGui);
         return;
 
     case 0x3006:
-        switch (subidx)
-        {
-        case 0:
-            //lout << ddev(MLog::eGui);
-            gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
-            //lout << edev(MLog::eGui);
-            return;
-
-        case 1:
-            /*
-             lout << ddev(MLog::eGui);
-             lout << "- Velocity set (" << GetNodeName() << ")" << endl;
-             lout << edev(MLog::eGui);
-             */
-            return;
-        }
-        break;
+        gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
+        return;
+
+    case 0x3007:
+        //gLog << inf2 << "- Velocity set (" << GetNodeName() << ")" << endl;
+        return;
 
     case 0x4000:
@@ -292 +262 @@
 
     case 0x6000:
-        //lout << ddev(MLog::eGui);
         gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
-        //lout << edev(MLog::eGui);
         return;
 
     case 0x6002:
-        //lout << ddev(MLog::eGui);
-        gLog << inf2 << "- " << GetNodeName() << ": Velocitz resolution set." << endl;
-        //lout << edev(MLog::eGui);
+        gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution set." << endl;
         return;
 
     case 0x6004:
-        switch (subidx)
-        {
-        case 0:
-            //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;
-
-        case 1:
-            //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;
-        }
-        break;
-
-
-    }
+        gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
+        fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
+        return;
+
+    case 0x6005:
+        gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
+        fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
+        return;
+    }
+
     NodeDrv::HandleSDOOK(idx, subidx, data, tv);
 }
@@ -348 +304 @@
 }
 
-void Dkc::StartNode()
-{
-    //
-    // Switch node from pre-operational state to operational state
-    // (This is not CANOpen compatible)
-    // After this the MACS will react on real movement commands.
-    //
-    gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
-    SendSDO(0x1000, 1, (LWORD_t)1);
-    WaitForSdo(0x1000, 1);
-}
-
 void Dkc::Arm()
 {
-    StartNode();
+    gLog << inf2 << "- " << GetNodeName() << ": Arming node." << endl;
+    SendSDO(0x1000, (LWORD_t)1);
+    WaitForSdo(0x1000);
 }
 
 void Dkc::Disarm()
 {
-    gLog << inf2 << "- " << GetNodeName() << ": Stopping Node." << endl;
-    SendSDO(0x1000, 1, (LWORD_t)0);
-    WaitForSdo(0x1000, 1);
+    gLog << inf2 << "- " << GetNodeName() << ": Disarming Node." << endl;
+    SendSDO(0x1000, (LWORD_t)0);
+    WaitForSdo(0x1000);
 }
 
@@ -385 +331 @@
     //
     gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
-    RequestSDO(0x1003, 2);
-    WaitForSdo(0x1003, 2);
+    RequestSDO(0x1003);
+    WaitForSdo(0x1003);
 
     /*
@@ -404 +350 @@
     ReqVelMax(); // Init fVelMax
 
+    // Request to send all PDOs at least once
+    ReqPDOs();
+
 #ifdef EXPERT
-    StartNode();
+    Arm();
 #endif
 
     gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
-    RequestSDO(0x1000, 1);
-    WaitForSdo(0x1000, 1);
+    RequestSDO(0x1000);
+    WaitForSdo(0x1000);
 }
 
@@ -422 +371 @@
 }
 
+void Dkc::ReqPDOs()
+{
+    gLog << inf2 << "- " << GetNodeName() << ": Requesting all PDOs." << endl;
+
+    SendSDO(0x1001, (LWORD_t)1);
+    WaitForSdo(0x1001);
+}
+
 void Dkc::ReqPos1()
 {
     gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 1." << endl;
-    RequestSDO(0x6004, 0);
-    WaitForSdo(0x6004, 0);
+    RequestSDO(0x6004);
+    WaitForSdo(0x6004);
 }
 
@@ -432 +389 @@
 {
     gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 2." << endl;
-    RequestSDO(0x6004, 1);
-    WaitForSdo(0x6004, 1);
+    RequestSDO(0x6005);
+    WaitForSdo(0x6005);
 }
 
@@ -460 +417 @@
 {
     gLog << dbg << "- Setting acceleration to: " << acc << endl;
-    SendSDO(0x2003, 0, acc);  // acceleration
-    WaitForSdo(0x2003, 0);
-}
-/*
-void Dkc::SetDeceleration(LWORD_t dec)
-{
-    gLog << dbg << "- Setting deceleration to: " << dec << endl;
-    SendSDO(0x2003, 1, dec);
-    WaitForSdo(0x2003, 1);
-}
-*/
+    SendSDO(0x2003, acc);  // acceleration
+    WaitForSdo(0x2003);
+}
+
 void Dkc::SetRpmMode(BYTE_t mode)
 {
@@ -476 +426 @@
     // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
     //
-    SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
-    WaitForSdo(0x3006, 0);
+    SendSDO(0x3006, mode ? string('s','t','r','t') : string('s','t','o','p'));
+    WaitForSdo(0x3006);
 }
 
 void Dkc::SetRpmVelocity(LWORDS_t cvel)
 {
-    SendSDO(0x3006, 1, (LWORD_t)cvel);
-    WaitForSdo(0x3006, 1);
+    SendSDO(0x3007, (LWORD_t)cvel);
+    WaitForSdo(0x3007);
 }
 
@@ -496 +446 @@
 
     gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
-    SendSDO(0x6004, 1, (LWORD_t)pos);
+    SendSDO(0x6005, (LWORD_t)pos);
     fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
 }
@@ -510 +460 @@
 
     gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl;
-    SendSDO(0x6004, 0, (LWORD_t)pos);
+    SendSDO(0x6004, (LWORD_t)pos);
     fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
 }
@@ -526 +476 @@
 
     gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << p << " ticks." << endl;
-    SendSDO(0x6004, 0, p);
+    SendSDO(0x6004, p);
     fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
-}
-
-void Dkc::StartVelSync()
-{
-    //
-    // The syncronization mode is disabled by a 'MOTOR STOP'
-    // or by a positioning command (POSA, ...)
-    //
-    gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
-    SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
-    WaitForSdo(0x3007, 0);
-}
-
-void Dkc::StartPosSync()
-{
-    //
-    // The syncronization mode is disabled by a 'MOTOR STOP'
-    // or by a positioning command (POSA, ...)
-    //
-    gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
-    SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
-    WaitForSdo(0x3007, 1);
 }
 

trunk/MagicSoft/Cosy/devdrv/dkc.h

@@ -73 +73 @@
 public:
     Dkc(const BYTE_t nodeid, const char *name=NULL);
-    virtual ~Dkc();
+    virtual ~Dkc() { }
 
     static LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
@@ -84 +84 @@
                  BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0);
 
+    void ReqPDOs();
     void ReqPos1();
     void ReqPos2();
@@ -91 +92 @@
     void ReqVelMax();
     void SetAcceleration(LWORD_t acc);
-    void SetDeceleration(LWORD_t dec);
+//    void SetDeceleration(LWORD_t dec);
     void SetVelocity(LWORD_t vel);
     void SetVelocityRel(Double_t vel);
     void SetRpmMode(BYTE_t mode=TRUE);
     void SetRpmVelocity(LWORDS_t cvel);
-    void SetPDO1On(BYTE_t flag=TRUE);
-
-    void StartVelSync();
-    void StartPosSync();
 
     void StartRelPos(LWORDS_t pos);
     void StartAbsPos(LWORDS_t pos);
     void StartAbsPosRev(Double_t pos);
-
-    void StartNode();
 
     bool IsArmed() const { return fArmed; }