Changeset 8875 for trunk/MagicSoft/Cosy/aposs

Timestamp:

02/24/08 11:26:09 (17 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/aposs/Magic.m (modified) (12 diffs)

trunk/MagicSoft/Cosy/aposs/Magic.m

@@ -216 +216 @@
 /* OLD ORIGINAL VALUES: Prop=100, Div=300, Int=1000 */
 if (get cannr==1) then
-SET KPROP  950
-SET KDER  2000
-SET KINT     0
+SET KPROP  100 /*950*/
+SET KDER   300 /*2000*/
+SET KINT  1000 /*0*/
 elseif (get cannr==2) then
 SET KPROP  100
@@ -329 +329 @@
 kIoModule = 4*256                    
 
-ON INT -(kIoModule+6) GOSUB DOOR_STATUS_CHANGED
-ON INT  (kIoModule+6) GOSUB DOOR_STATUS_CHANGED
-
 /*-------------------------------------------------------------------------*/
 /* Init                                                                    */
@@ -350 +347 @@
    
    init  = 0
+
+   oldstate = kTRUE
 
    gosub reset
@@ -361 +360 @@
       else
          fuse  = in (kIoModule+1)
-         emcy  = in (kIoModule+2)
+         emcy  = in (kIoModule+2)                             
          vltg  = in (kIoModule+3)
          mode  = in (kIoModule+4)
          door  = in (kIoModule+6)
          ready = in 1
-
+                                     
          if (door==0) then
             preop = kTRUE
-         endif
-
-         if (preop==kTRUE) then
-            print "Door open!"
-            gosub reset
-         elseif (ready==0) and (RF==1) then
+         endif                         
+
+         if (ready==0) and (RF==1) then
             print "DKC not ready, but RF set... setting RF=AH=0!"
             gosub reset
@@ -438 +434 @@
          endif         
       endif
-
+       
+      if (preop==kTRUE and oldstate!=preop) then
+          print "Door switch changed state!"
+          gosub reset
+      endif
+      oldstate = preop
+       
       rc = CANIN sdorx -1 0 canhi canlo
    goto mainloop
@@ -455 +457 @@
    SUBPROG reset
       init = 0
-      
+                
       out 1 0
       out 2 0   
@@ -461 +463 @@
       motor off    
       waitt 1000
-   
+
       if (brake==1 and get cannr==3) then         
          waitt 3000          /* wait 3s for DKC to stop the motor */
@@ -467 +469 @@
          waitt 1000
       endif   
-      
+
       canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1))
    return
@@ -482 +484 @@
       PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
 */
-      if (idx==0x1000 and subidx==1) then
-         if (sdoval==1) then
-            if (door==0) then
-               canlo = 1
-            else
-               preop = kFALSE
-            endif
-         elseif
-            preop = kTRUE
+      if (idx==0x1000 and subidx==1) then   
+         preop = kTRUE
+         if (sdoval==1 and door==1) then
+             preop = kFALSE
+         else
+             canlo = 1
          endif
       elseif (idx==0x1003 and subidx==0 and sdoval==0) then
@@ -537 +536 @@
       elseif (idx == 0x2003) then
         if (subidx) then
-          DEC sdoval
+          /* If velocity mode active */
+          /*if (AXEND&4)
+             DEC sdoval%4
+          else*/
+             DEC sdoval
+          /*endif*/
         else
-          ACC sdoval
+          /*if (AXEND&4)
+             ACC sdoval%4
+          else*/
+             ACC sdoval
+          /*endif*/
         endif 
       elseif (idx == 0x3000) then
@@ -584 +592 @@
          elseif (subidx == 1) then
             /* It is unclear where this arbitraty factor comes from */
-            CVEL sdoval/4
+            CVEL sdoval%4
          endif 
       elseif (idx == 0x3007) then
@@ -810 +818 @@
 /*-------------------------------------------------------------------------*/
    SUBPROG PROC_PDO1
-      CANOUT pdo1 (AXEND|(preop<<9)) APOS
+      CANOUT pdo1 (AXEND|(((~preop)&1)<<8)) APOS
    RETURN