Changeset 1807 for trunk/MagicSoft/Cosy/aposs/Manual.m

Timestamp:

03/06/03 17:53:59 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/aposs/Manual.m (modified) (8 diffs)

trunk/MagicSoft/Cosy/aposs/Manual.m

@@ -4 +4 @@
 /*                                                                         */
 kVERSION    = 0   /*                                                       */
-kSUBVERSION = 3   /*                                                       */
+kSUBVERSION = 6   /*                                                       */
 /*                                                                         */
 /*  HISTORY:                                                               */
+/*                                                                         */
+/*   * V0.6                                                                */
+/*       - do not reset the velocity if set already                        */
+/*       - set acceleration to 20%                                         */
+/*                                                                         */
+/*   * V0.5:                                                               */
+/*       - changes 'DKC Ready' signal to IN1 for all MACS                  */
+/*       - control brake only when cannr==3                                */
+/*                                                                         */
+/*   * V0.4:                                                               */
+/*       - restart MACS with PRGPAR 1 when not in manual mode              */
+/*       - replaced label reset by a subprg                                */
 /*                                                                         */
 /*   * V0.3:                                                               */
@@ -112 +124 @@
 /*----------------*/
 SET RAMPTYPE 1                    /* Ramp: 0=linear, 1=sinus     */
-defacc = 60*vres%100
+defacc = 10*vres%100
 
 SET DFLTACC defacc                /* Default acceleratio: [%]    */
@@ -124 +136 @@
 
 print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min"
-print "V_man: ", manvel, " Encoder Ticks/min"
+print "V_man: ", manvel, " Encoder Ticks/min,  Acc=", defacc
 
 /*----------------*/
@@ -152 +164 @@
                               
 brake = 0
+RF    = 0
+velo  = 0
    
-reset:   
-   out 1 0
-   out 2 0   
-   RF = 0
-   motor off    
-   waitt 1000
-   
-   if (brake==1) then         
-      waitt 3000          /* wait 3s for DKC to stop the motor */
-      out (kIoModule+1) 0 /* brake the brake                   */
-      waitt 1000
-   endif   
+gosub reset
          
 mainloop:       
@@ -171 +174 @@
    vltg  = in (kIoModule+3)
    mode  = in (kIoModule+4)   
-   
+   /*
    if (get cannr==1) or (get cannr==2) then
-       ready = in 1
+   */
+   ready = in 1
+   /*
    elseif (get cannr==3) then
-       ready = in (kIoModule+5)  /* !!!FIXME!!! */
+       ready = in (kIoModule+5)
    endif   
+   */
        
    if (ready==0) and (RF==1) then
       print "DKC not ready, but RF set... setting RF=AH=0!"
-      goto reset
+      gosub reset
+      goto mainloop
+   elseif mode==0 then
+      print "Control not in manual mode!"
+      gosub reset  
+      SET PRGPAR 1
+      exit
    elseif fuse==0 then 
       print "Motor-Power Fuse not OK!"
-      goto reset
+      gosub reset
+      goto mainloop
    elseif vltg==0 then 
       print "Overvoltage control broken!"
-      goto reset
+      gosub reset
+      goto mainloop
    elseif emcy==0 then 
       print "Please release Emergency Stop!"
-      goto reset
-   elseif mode==0 then
-      print "Control not in manual mode!"
-      goto reset
+      gosub reset
+      goto mainloop
    elseif (ready==1) and (RF==0) then
       print "DKC powered, RF=0... setting RF=AH=1!"
@@ -210 +222 @@
       waitt 100
 
-      if (brake==0) then
+      if (brake==0 and get cannr==3) then
          out (kIoModule+1) 1
          brake = 1
@@ -239 +251 @@
    backward = in 3       
                             
-   if     (forward==1) and (backward==0) then
-      cvel  manvel             
-   elseif (forward==0) and (backward==1) then
+   if     (forward==1) and (backward==0) and (velo!=manvel) then
+      cvel  manvel                                          
+      velo = manvel
+   elseif (forward==0) and (backward==1) and (velo!=-manvel) then
       cvel -manvel
-   else
-      cvel 0       
+      velo = -manvel
+   elseif (forward==backward) and (velo!=0) then
+      cvel 0
+      velo = 0       
    endif              
 goto mainloop       
        
 SUBMAINPROG 
+   subprog reset
+      out 1 0
+      out 2 0   
+      RF = 0
+      motor off    
+      velo = 0
+      waitt 1000
+   
+      if (brake==1 and get cannr==3) then         
+         waitt 3000          /* wait 3s for DKC to stop the motor */
+         out (kIoModule+1) 0 /* brake the brake                   */
+         waitt 1000
+      endif   
+   return
+      
    subprog suberror
       out 1 0
@@ -254 +284 @@
       RF = 0
       waitt 100
+      velo = 0
       
-      if (brake==1) then
+      if (brake==1 and get cannr==3) then
          waitt 5000
          out (kIoModule+1) 0