Changeset 808 for trunk/MagicSoft/Cosy/devdrv

Timestamp:

05/25/01 16:51:24 (24 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/devdrv

Files:

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

trunk/MagicSoft/Cosy/devdrv/macs.cc

@@ -10 +10 @@
     : NodeDrv(nodeid, out), fMacId(2*nodeid+1),
     fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
-    fPosActive(0), fRpmActive(0), fError(FALSE)
+    fPosActive(0), fRpmActive(0)
 {
 }
@@ -115 +115 @@
     RequestSDO(0x2004);
     WaitForSdo(0x2004);
-
-
-*/
+    */
+
     lout << "- Requesting Mac Software Version of " << (int)GetId() << endl;
     RequestSDO(0x100a);
     WaitForSdo(0x100a);
-    //    
+
     SetRpmMode(FALSE);
 
@@ -131 +130 @@
 
 
-    SetHome(250000);
+//    SetHome(250000);
 
 //    lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
@@ -195 +194 @@
     WaitForSdo(0x6003, 2);
 
+    // home also defines the zero point of the system
     SendSDO(0x3001, string('h','o','m','e'));       // home
     WaitForSdo(0x3001);
     lout << "- Home position of #" << (int)GetId() << " reached. " << endl;
+
+    SendSDO(0x6003, 0, string('s','e','t'));       // home
+    WaitForSdo(0x6003, 0);
 }
 
@@ -208 +211 @@
 void Macs::SetAcceleration(LWORD_t acc)
 {
-    SendSDO(0x2003, 0, acc);     // acceleration
+    SendSDO(0x2003, 0, acc);  // acceleration
     WaitForSdo(0x2003, 0);
 }
@@ -214 +217 @@
 void Macs::SetDeceleration(LWORD_t dec)
 {
-    SendSDO(0x2003, 1, dec);     // acceleration
+    SendSDO(0x2003, 1, dec);  // acceleration
     WaitForSdo(0x2003, 1);
 }
@@ -223 +226 @@
     // 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);
@@ -250 +252 @@
     WaitForSdo(0x3008);
 }
+
+void Macs::SetSyncMode()
+{
+    lout << "- Setting Sync Mode #" << (int)GetId() << endl;
+    SendSDO(0x3007, string('S', 'Y', 'N', 'C'));
+    WaitForSdo(0x3007);
+}
 /*
 void Macs::ReqAxEnd()
@@ -283 +292 @@
     LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
 
+    //
+    // errnum==0 gives a sudden information that something happened. Now the
+    // microcontroller is running inside its interrup procedure which
+    // stopped the normal program. The interrupt procedure should try to clear
+    // the error state of the hardware. This should never create a new error!
+    //
     if (!errnum)
     {
         cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl;
-        fError = TRUE;
+        SetError(-1);
         return;
     }
+
+    //
+    // Now the error is handled by the hardware now it is the software part
+    // to react on it. The Error flag now is set to the correct value.
+    //
+    if (GetError()>0)
+        cout << "Mac #" << (int)GetId() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;
+
+    SetError(errnum);
 
     cout << "Mac #" << (int)GetId() << " reports: ";
@@ -305 +329 @@
         return;
 
+    case 11:
     case 25:
         switch (errinf)
         {
+        case -1:
+            cout << "Negative";
+            break;
         case 1:
             cout << "Positive";
-            break;
-        case 2:
-            cout << "Negative";
             break;
         default:
             cout << "-unknown-";
         }
-        cout << " endswitch activated." << endl;
-        fError = FALSE;
+        switch (errnum)
+        {
+        case 11:
+            cout << " software endswitch activated." << endl;
+            break;
+        case 25:
+            cout << " hardware endswitch activated." << endl;
+            break;
+        }
         return;
 
@@ -330 +362 @@
 }
 
+void Macs::HandleError()
+{
+    //
+    // If there is no error we must not handle anything
+    //
+    if (!HasError())
+        return;
+
+    //
+    // If the program got into the: HandleError state before the hardware
+    // has finished handeling the error we have to wait for the hardware
+    // handeling the error
+    //
+    // FIXME: Timeout???
+    //
+    while (GetError()<0)
+        usleep(1);
+
+    //
+    // After this software and hardware should be in a state so that
+    // we can go on working 'as usual' Eg. Initialize a Display Update
+    //
+    cout << "Mac #" << (int)GetId() << " Handling Error #" << GetError() << endl;
+    switch (GetError())
+    {
+    case  6: // home
+    case  8: // control dev
+    case  9: // zero idx
+    case 84: // ON TIME
+        // Stop program?
+        return;
+
+    case 11: // software endswitch
+    case 25: // hardware endswitch
+        DelError();
+        return;
+    }
+}
+
 double Macs::GetTime()
 {
@@ -350 +421 @@
 }
 
+/*   0x2000 0 rw Maximum positioning error     */
+/*          1 rw Negative Software Endswitch   */
+/*          2 rw Positive Software Endswitch   */
+void Macs::SetNegEndswitch(LWORDS_t val)
+{
+    SendSDO(0x2000, 1, (LWORD_t)val);
+    WaitForSdo(0x2000, 1);
+}
+
+void Macs::SetPosEndswitch(LWORDS_t val)
+{
+    SendSDO(0x2000, 2, (LWORD_t)val);
+    WaitForSdo(0x2000, 2);
+}
+
+void Macs::EnableEndswitches(bool neg, bool pos)
+{
+    SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
+    WaitForSdo(0x2000, 3);
+}

trunk/MagicSoft/Cosy/devdrv/macs.h

@@ -21 +21 @@
     BYTE_t   fPosActive;
     BYTE_t   fRpmActive;
-
-    int      fError;      // Indicater for error state
 
     LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
@@ -58 +56 @@
     void SetVelocity(LWORD_t vel);
     void SetNoWait(BYTE_t flag=TRUE);
+    void SetSyncMode();
     void SetRpmMode(BYTE_t mode=TRUE);
     void SetRpmVelocity(LWORDS_t cvel);
     void SetPDO1On(BYTE_t flag=TRUE);
+    void SetPosEndswitch(LWORDS_t val);
+    void SetNegEndswitch(LWORDS_t val);
+
+    void EnableEndswitches(bool neg=true, bool pos=true);
 
     void StartRelPos(LWORDS_t pos);
@@ -79 +82 @@
     LWORDS_t GetPos()    { return fPos; }
     LWORDS_t GetVel()    { return fVel; }
-    LWORD_t  GetVelRes() { return fVelRes; }
+    LWORD_t  GetVelRes() { return fVelRes; } // Velocity units (would be 100 for %)
 
-    int      HasError() { return fError; }
+    void HandleError();
 };