Changeset 1807 for trunk/MagicSoft/Cosy


Ignore:
Timestamp:
03/06/03 17:53:59 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/aposs
Files:
1 added
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/aposs/Magic.m

    r1134 r1807  
    1010/*                                                                         */
    1111kVERSION    = 0   /*                                                       */
    12 kSUBVERSION = 63  /*                                                       */
     12kSUBVERSION = 67  /*                                                       */
    1313/*                                                                         */
    1414/*  HISTORY:                                                               */
     15/*                                                                         */
     16/*   * V0.67:                                                              */
     17/*       - trying to implement the control cabinet stuff                   */
     18/*                                                                         */
     19/*   * V0.66:                                                              */
     20/*       - implemented canopen node guarding (0x4000)                      */
     21/*       - implemented some kind of host guarding (in conjuction with      */
     22/*         the implemented nodeguarding)                                   */
     23/*       - removed old guarding using SDO 0x4000                           */
     24/*       - implemented SDO 0x100c (guardtime)                              */
     25/*       - implemented SDO 0x100d (lifetimefactor)                         */
     26/*                                                                         */
     27/*   * V0.65:                                                              */
     28/*       - fixed a bug in the handling of the endswitches                  */
     29/*                                                                         */
     30/*   * V0.64:                                                              */
     31/*       - removed FORCEHOME                                               */
    1532/*                                                                         */
    1633/*   * V0.63:                                                              */
    1734/*       - added movement handshake timeout (removed 0x400 WAITAX)         */
    1835/*       - added brackets around string SDOs                               */
    19 /*       - changed SDO 0x3007 to support both sync modes
     36/*       - changed SDO 0x3007 to support both sync modes                   */
    2037/*                                                                         */
    2138/*   * V0.62:                                                              */
    … …  
    6784/* ----------------------------------------------------------------------- */
    6885
    69 PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION /*                        */
     86PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION, " (CANbus controlled)" /**/
    7087
    7188/* ----------------------------------------------------------------------- */
    … …  
    8097/*   0x100a x ro Software Version                                          */
    8198/*   0x100b x ro Node number                                               */
     99/*   0x100c x rw Guardtime (ms)                                            */
     100/*   0x100d x rw Lifetime factor                                           */
    82101/*   0x100e x ro COB ID for Guarding                                       */
    83102/*   0x1010 x wo Write data to EEPROM                                      */
    … …  
    100119/*          1 wo Position Syncronisation 'sync'                            */
    101120/*   0x3008 x wo Nowait 'on', 'off'                                        */
    102 /*   0x4000 0 wo Reset timeout timer                                       */
    103 /*          1 rw Enable timeout timer, 'on' (1), 'off'(0)                  */
    104 /*          2 rw Timeout timer time                                        */
     121/*   0x4000 0 wo Reset timeout timer (Nodeguard)                           */
    105122/*   0x6000 x rw Rotation Direction                                        */
    106123/*   0x6002 x rw Velocity Resolution                                       */
    … …  
    121138/* section for global constants                                            */
    122139/*-------------------------------------------------------------------------*/
    123 SET PRGPAR        0          /* Restart Program on Exit                    */
     140SET PRGPAR       -1          /* Don't restart any Program on Exit          */
    124141
    125142SET ENCODERTYPE   0          /* Incremental Encoder                        */
    126 SET ENCODER     500          /* Encoder has 500 Ticks                      */
    127143SET MENCODERTYPE  0          /* Incremental Encoder (Master)               */
    128 SET MENCODER    500          /* Encoder has 500 Ticks (Master)             */
    129 SET VELMAX     3600          /* Motor: Maximum rounds per minute           */
    130 SET POSERR     1500          /* Maximum tolarable Position error (qc) 0.1° */
    131 SET ENDSWMOD      1          /* At End Switch Stop Motor with Max Decel.   */
    132 SET ERRCOND       2          /* Motor Stop                                 */
    133 SET POSDRCT       1          /* rotation direction                         */
     144
     145SET ENDSWMOD      0          /* No End Switch                              */
     146SET ERRCOND       2          /* Motor Stop, position control, no break     */
     147SET POSDRCT      -1          /* rotation direction                         */
     148/* OLD: SET POSDRCT      -1          /* rotation direction                         */
    134149SET POSFACT_Z     1          /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc  */
    135150SET POSFACT_N     1          /*                                            */
    136 SET HOME_FORCE    1          /* Force Home positioning on startup          */
     151
     152SET HOME_FORCE    0          /* Don't force Home positioning on mainloopup    */
    137153SET HOME_OFFSET   0          /* Offset between index and home position     */
    138154SET HOMETYPE      0          /* drive to home, reverse, go to next index   */
    139 SET RAMPTYPE      0          /* Ramp Type: 0=Trapez, 1=Sinus               */
    140155
    141156/*----------------*/
    142157/* syncronisation */
    143158/*----------------*/                                                       
    144 SET MENCODERTYPE   0         /* Incremental Encoder (Master)               */
    145 SET MENCODER     500         /* Encoder has 500 Ticks (Master)             */
    146159SET SYNCFACTM      1         /* Master Sync Velocity factor                */
    147160SET SYNCFACTS      1         /* Slave Sync Velocity factor                 */
    … …  
    153166/*    Inputs      */
    154167/*----------------*/
    155 SET I_REFSWITCH  -2          /* Reference Switch, Input 2, leading edge    */
    156 SET I_POSLIMITSW -2          /* Pos Limit Switch, Input 2, leading edge    */
    157 SET I_NEGLIMITSW -1          /* Neg Limit Switch, Input 1, leading edge    */
     168SET I_REFSWITCH   0          /* Reference Switch                           */
     169SET I_POSLIMITSW  0          /* Pos Limit Switch                           */
     170SET I_NEGLIMITSW  0          /* Neg Limit Switch                           */
    158171SET I_BREAK       0          /* Input which brakes a running program       */
    159172SET I_CONTINUE    0          /* Input to continue a broken program         */
    … …  
    168181
    169182/*----------------*/
     183/* Unit param.    */
     184/*----------------*/
     185SET RAMPTYPE      1          /* Ramp Type: 0=Trapez, 1=Sinus               */
     186/* OLD: SET RAMPTYPE      0          /* Ramp Type: 0=Trapez, 1=Sinus               */
     187SET ENCODER    1500          /* Encoder has 1500 Ticks                     */
     188SET MENCODER   1500          /* Encoder has 500 Ticks (Master)             */
     189SET VELMAX     3000          /* Motor: Maximum revolutions per minute      */
     190SET POSERR     1500          /* Maximum tolarable Position error (qc) 0.1° */
     191SET RAMPMIN   10000          /* Shortest Ramp 10s                          */
     192
     193/*----------------*/
    170194/* Dflt vel & acc */
    171 /*----------------*/
     195/*----------------*/
     196
     197/* Prop=100, Div=300, Int=800 */
     198if (get cannr==1) then
     199SET KPROP 100
     200SET KDER  300
     201SET KINT  1000
     202elseif (get cannr==2) then
     203SET KPROP 100
     204SET KDER  200
     205SET KINT  150
     206else
     207SET KPROP 350
     208SET KDER  50
     209SET KINT  350
     210endif
     211
    172212vres = (GET ENCODER)*(GET VELMAX)           /*  ticks/R * R/M = ticks/min  */
    173213SET VELRES    vres                          /* Set velocity units          */
    174 SET HOME_VEL  -(25*vres%100)                /* Home position velocity: 25% */
    175 SET HOME_RAMP  (25*vres%100)                /* Home position accel: 25%    */
    176 SET DFLTACC    (10*vres%100)                /* Default acceleratio: 10%    */
    177 SET DFLTVEL    (10*vres%100)                /* Default velocity: 10%       */
     214
     215
     216/* OLD: SET HOME_VEL  -(25*vres%100)                /* Home position velocity: 25% */
     217/* OLD: SET HOME_RAMP  (25*vres%100)                /* Home position accel: 25%    */
    178218
    179219/*----------------*/
    180 /* Software range */
     220/* Manual control */
    181221/*----------------*/
    182 SET SWPOSLIMACT   0             /* positive software limit switch inactive */
    183 SET SWNEGLIMACT   0             /* negative software limit switch inactive */
    184 SET POSLIMIT      0             /* positive software limit (qc)            */
    185 SET NEGLIMIT      0             /* negative software limit (qc)            */
     222SET RAMPTYPE 1                    /* Ramp: 0=linear, 1=sinus     */
     223defacc = 60*vres%100
     224
     225SET DFLTACC defacc                /* Default acceleratio: [%]    */
     226ACC defacc
     227DEC defacc*2
     228                                  /* Velocity which is reached in   
     229                                     a time given by RAMPMIN     */                                     
     230SET DFLTVEL    (1*vres%100)       /* Default velocity [%]        */ 
     231
     232/*manvel = (4*vres%100)  /* 150 U/min */     /* Max speed in man mode: [%]  */
     233
     234print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min"
     235/*print "V_man: ", manvel, " Encoder Ticks/min"*/
    186236
    187237/*-------------------------------------------------------------------------*/
    … …  
    194244pdo1on  = kFALSE
    195245
    196 timeout     = 100
    197 timeouton   = kFALSE   
    198 timeouttime = TIME
     246guardtime      = 0
     247lifetimefactor = 0
     248timeouttime    = TIME
     249firsttimeout   = 0
    199250
    200251/*-------------------------------------------------------------------------*/
    … …  
    203254/* The CAN Object are static object. This is why they must be deleted.     */
    204255/* The program should run in any of our nodes.                             */
    205 /* Therefor the standard CAN objects (SDO, PDO1, PDO2) for communication   */
     256/* Therefor the standard CAN objects (SDo, PDO1, PDO2) for communication   */
    206257/* are defined. The nodenumber is part of the object ID (this is somehow   */
    207258/* similar to the TCP/IP ports)                                            */
    208259/*-------------------------------------------------------------------------*/
    209 CANDEL -1
     260CANDEL -1 
     261
    210262nodenr = GET CANNR
    211263PRINT "Initializing Node Nr.", nodenr
    212 pdo1  = DEFCANOUT (0x180+nodenr) 8
    213 pdo2  = DEFCANOUT (0x280+nodenr) 8
    214 sdotx = DEFCANOUT (0x580+nodenr) 8
    215 sdorx = DEFCANIN  (0x600+nodenr) 8
     264pdo1    = DEFCANOUT (0x180+nodenr) 8
     265pdo2    = DEFCANOUT (0x280+nodenr) 8
     266pdo3    = DEFCANOUT (0x380+nodenr) 8
     267sdotx   = DEFCANOUT (0x580+nodenr) 8
     268sdorx   = DEFCANIN  (0x600+nodenr) 8
     269/*
     270        guardrx = DEFCANIN  (0x700+nodenr) 8
     271        guardtx = DEFCANOUT (0x700+nodenr) 8
     272        */
    216273                               
    217274/* Close and reopen communication, enable buffering */                               
    218275err = REOPEN 0 0
    219276
    220 /*-------------------------------------------------------------------------*/
    221 /* Init                                                                    */
    222277/*-------------------------------------------------------------------------*/
    223278/* Before the motor control hardware is enabled (hi on output 1)           */
    … …  
    226281/* of the program.                                                         */
    227282/*-------------------------------------------------------------------------*/
    228 MOTOR  STOP
     283/*MOTOR  STOP
    229284MOTOR  OFF
    230285CVEL   0
     286OUT    1 1
     287*/
    231288NOWAIT OFF
    232 OUT    1 1
    233289
    234290/*-------------------------------------------------------------------------*/
    … …  
    249305ON ERROR GOSUB PROC_ERROR
    250306
     307/***************************************************************************/
     308kIoModule = 4*256                   
     309
     310/*-------------------------------------------------------------------------*/
     311/* Init                                                                    */
    251312/*-------------------------------------------------------------------------*/
    252313/* Program Main Loop                                                       */
    … …  
    261322   canlo = 1
    262323
     324         brake = 0
     325   RF    = 0     
     326   
     327   init  = 0
     328
     329   gosub reset
     330
    263331   PRINT "Starting Mainloop..."
    264    
     332
     333   rc = 1
    265334   MAINLOOP:
    266       rc = CANIN sdorx 0 0 canhi canlo
    267335      if (rc==0) then      /* It must be tested because ON PERIOD breaks 'wait for obj' */
    268336         gosub PROC_SDORX
     337      else
     338         fuse  = in (kIoModule+1)
     339         emcy  = in (kIoModule+2)
     340         vltg  = in (kIoModule+3)
     341         mode  = in (kIoModule+4)   
     342         ready = in 1
     343
     344         if (ready==0) and (RF==1) then
     345            print "DKC not ready, but RF set... setting RF=AH=0!"
     346            gosub reset
     347            goto mainloop
     348         elseif mode==1 then
     349            print "Control not in PC mode!"
     350            gosub reset
     351            SET PRGPAR 0
     352            exit
     353         elseif fuse==0 then
     354            print "Motor-Power Fuse not OK!"
     355            gosub reset
     356            goto mainloop
     357         elseif vltg==0 then
     358            print "Overvoltage control broken!"
     359            gosub reset
     360            goto mainloop
     361         elseif emcy==0 then
     362            print "Please release Emergency Stop!"
     363            gosub reset
     364            goto mainloop
     365         elseif (ready==1) and (RF==0) then
     366            print "DKC powered, RF=0... setting RF=AH=1!"
     367            /*
     368             * After switching on power wait at least 300ms until
     369             * control changed state 'bb' to 'ab'
     370             */
     371            cvel 0
     372            waitt 300
     373            out 1 0
     374            out 2 0
     375            motor off     
     376            waitt 100
     377            out 1 1
     378            out 2 1                                 
     379            RF = 1
     380            waitt 100
     381
     382            if (brake==0 and get cannr==3) then
     383               out (kIoModule+1) 1
     384               brake = 1
     385               waitt 1000
     386            endif         
     387           
     388            motor on
     389
     390            canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<<5) | (brake<<6)) 0
     391          /*         
     392            if (get cannr==2) then
     393               syncv
     394               print "Synchronizing speed..."             
     395            else   
     396               print "Starting CAN mode..."     ^       
     397               goto CANSTART
     398            endif   */
     399         elseif (ready==0) or (RF==0) then
     400            print "No Power, no RF..."
     401            canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<5) | (brake<<6)) 0
     402            waitt 500
     403            goto mainloop
     404         endif         
    269405      endif
    270    goto MAINLOOP
     406
     407      rc = CANIN sdorx -1 0 canhi canlo
     408   goto mainloop
     409
    271410ENDMAIN:
    272411   MOTOR STOP
    … …  
    280419
    281420SUBMAINPROG
     421   SUBPROG reset
     422      init = 0
     423     
     424      out 1 0
     425      out 2 0   
     426      RF = 0
     427      motor off   
     428      waitt 1000
     429   
     430      if (brake==1 and get cannr==3) then         
     431         waitt 3000          /* wait 3s for DKC to stop the motor */
     432         out (kIoModule+1) 0 /* brake the brake                   */
     433         waitt 1000
     434      endif   
     435     
     436      canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1))
     437   return
     438     
    282439   /*----------------------------------*/
    283440   /* PROC_CANOPENMSG                  */
    … …  
    290447      PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
    291448*/
    292       if (idx == 0x1003 and subidx == 0 and sdoval == 0) then
     449      if (idx==0x1003 and subidx==0 and sdoval==0) then
    293450         i = 9
    294451         while (i) do
    … …  
    296453            i = i - 1
    297454         endwhile
     455      elseif (idx==0x100c) then
     456         guardtime = sdoval   
     457         ON PERIOD 0 GOSUB PROC_Timeout
     458         if (lifetimefactor>0 and guardtime>0) then
     459            timeouttime = TIME + guardtime*lifetimefactor
     460            ON PERIOD guardtime GOSUB PROC_Timeout
     461         endif
     462      elseif (idx == 0x100d) then
     463         lifetimefactor = sdoval   
     464         if (lifetimefactor==0) then
     465            ON PERIOD 0 GOSUB PROC_Timeout
     466         endif
    298467      elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then
    299468         SAVEPROM
    … …  
    380549         endif
    381550      elseif (idx == 0x4000) then
    382          if (subidx == 0) then         
    383             timeouttime = TIME + timeout                 
    384          elseif (subidx == 1) then
    385             ON PERIOD 0 GOSUB PROC_Timeout
    386             if (sdoval == ('o'<<24|'n'<<16)) then
    387                timeouttime = TIME + timeout
    388                ON PERIOD timeout GOSUB PROC_Timeout
    389                timeouton = kTRUE                               
    390             elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
    391                timeouton = kFALSE
    392             endif
    393          elseif (subidx == 2) then
    394             timeout = sdoval             
    395             if (timeouton) then
    396                ON PERIOD 0 GOSUB PROC_Timeout
    397                timeouttime = TIME + timeout
    398                ON PERIOD timeout GOSUB PROC_Timeout
    399             endif
    400          endif
    401 /*    elseif (idx == 0x4000 and
    402               (sdoval>>24)&0xff == 'S' and
    403               (sdoval>>16)&0xff == 'T' and
    404               (sdoval>>8) &0xff == 'O' and
    405               (sdoval)    &0xff == 'P') then
    406          CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
    407          goto ENDMAIN
    408 */    elseif (idx == 0x6000) then
     551            timeouttime = TIME + guardtime*lifetimefactor                 
     552      elseif (idx == 0x6000) then
    409553         if (sdoval&1) then
    410554            SET POSDRCT -1
    … …  
    485629      elseif (idx == 0x100a) then
    486630         sdoval = (kVERSION<<16) | kSUBVERSION
     631      elseif (idx == 0x100c) then
     632         sdoval = guardtime
     633      elseif (idx == 0x100d) then
     634         sdoval = lifetimefactor     
    487635      elseif (idx == 0x100e) then
    488          sdoval = 0x700 | nodenr
     636         sdoval = 0x600|nodenr /*0x700 | nodenr*/
    489637      elseif (idx == 0x1010) then
    490638         sdoval = 1
    … …  
    523671      elseif (idx == 0x2004) then
    524672         sdoval = STAT
    525       elseif (idx == 0x4000) then
    526          if (subidx == 1) then
    527             sdoval = timeouton
    528          elseif (subidx == 2) then
    529             sdoval = timeout               
    530          endif
    531673      elseif (idx == 0x6000) then
    532674         if (GET POSDRCT == 1) then
    … …  
    614756/* Timeout Interrupt                                                       */
    615757/*-------------------------------------------------------------------------*/
    616    SUBPROG PROC_Timeout                         
    617       diff = timeouttime - TIME
    618       if (diff < 0) then                                 
     758   SUBPROG PROC_Timeout
     759      if (TIME > timeouttime) then                                 
    619760         MOTOR STOP
    620          
     761               
     762         if (firsttimeout==0) then
     763                   
    621764         /* Tell the bus that an error occured */
    622765         CANOUT pdo2 0 0
    … …  
    633776
    634777         errinf = 0
    635                          
    636          /*if (firstcall)*/
    637             PRINT "User Timeout at uptime=", (TIME%1000), "s missed=", -diff, "ms"
    638          /*firstcall = kFALSE  */
     778         
     779         PRINT "User Timeout!"
    639780
    640781         /* tell the bus what exactly happened */
    641782         CANOUT pdo2 errlist[2] errinf
    642       /*elseif
    643          firstcall = kTRUE*/
     783         endif
     784         
     785         firsttimeout = 1
     786      else
     787         firsttimeout = 0
    644788      endif   
    645789   RETURN   
    … …  
    649793/*-------------------------------------------------------------------------*/
    650794   SUBPROG PROC_ERROR
    651       MOTOR STOP
     795      /*MOTOR STOP*/
     796      out 1 0
     797      out 2 0
     798      RF = 0
    652799
    653800      /* Tell the bus that an error occured */
    654801      CANOUT pdo2 0 0
     802     
     803      waitt 100
    655804     
     805      if (brake==1 and get cannr==3) then
     806         waitt 5000
     807         out (kIoModule+1) 0
     808         waitt 500
     809      endif         
     810
     811      print "Error #", errno
     812
    656813      i = errlist[1] + 1              /* Fill status of array       */
    657814      while (i>2) do                  /* shift errors by one        */
    … …  
    693850            PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS
    694851            SET SWPOSLIMACT 0
    695             ERRCLR
     852            ERRCLR           
    696853            CVEL  (vres%100)    /*   1% */
    697854            ACC   (10*vres%100) /*  10% */
    … …  
    730887            DEC   (10*vres%100)  /* 10% */
    731888            CSTART
    732             WHILE (IN poslsw == 0) DO ENDWHILE
     889            WHILE (IN neglsw == 0) DO ENDWHILE
    733890            CSTOP
    734             SET I_NEGLIMITSW -poslsw
     891            SET I_NEGLIMITSW -neglsw
    735892            errinf = -1
    736893         endif

  • trunk/MagicSoft/Cosy/aposs/Manual.m

    r1716 r1807  
    44/*                                                                         */
    55kVERSION    = 0   /*                                                       */
    6 kSUBVERSION = 3   /*                                                       */
     6kSUBVERSION = 6   /*                                                       */
    77/*                                                                         */
    88/*  HISTORY:                                                               */
     9/*                                                                         */
     10/*   * V0.6                                                                */
     11/*       - do not reset the velocity if set already                        */
     12/*       - set acceleration to 20%                                         */
     13/*                                                                         */
     14/*   * V0.5:                                                               */
     15/*       - changes 'DKC Ready' signal to IN1 for all MACS                  */
     16/*       - control brake only when cannr==3                                */
     17/*                                                                         */
     18/*   * V0.4:                                                               */
     19/*       - restart MACS with PRGPAR 1 when not in manual mode              */
     20/*       - replaced label reset by a subprg                                */
    921/*                                                                         */
    1022/*   * V0.3:                                                               */
    … …  
    112124/*----------------*/
    113125SET RAMPTYPE 1                    /* Ramp: 0=linear, 1=sinus     */
    114 defacc = 60*vres%100
     126defacc = 10*vres%100
    115127
    116128SET DFLTACC defacc                /* Default acceleratio: [%]    */
    … …  
    124136
    125137print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min"
    126 print "V_man: ", manvel, " Encoder Ticks/min"
     138print "V_man: ", manvel, " Encoder Ticks/min,  Acc=", defacc
    127139
    128140/*----------------*/
    … …  
    152164                             
    153165brake = 0
     166RF    = 0
     167velo  = 0
    154168   
    155 reset:   
    156    out 1 0
    157    out 2 0   
    158    RF = 0
    159    motor off   
    160    waitt 1000
    161    
    162    if (brake==1) then         
    163       waitt 3000          /* wait 3s for DKC to stop the motor */
    164       out (kIoModule+1) 0 /* brake the brake                   */
    165       waitt 1000
    166    endif   
     169gosub reset
    167170         
    168171mainloop:       
    … …  
    171174   vltg  = in (kIoModule+3)
    172175   mode  = in (kIoModule+4)   
    173    
     176   /*
    174177   if (get cannr==1) or (get cannr==2) then
    175        ready = in 1
     178   */
     179   ready = in 1
     180   /*
    176181   elseif (get cannr==3) then
    177        ready = in (kIoModule+5)  /* !!!FIXME!!! */
     182       ready = in (kIoModule+5)
    178183   endif   
     184   */
    179185       
    180186   if (ready==0) and (RF==1) then
    181187      print "DKC not ready, but RF set... setting RF=AH=0!"
    182       goto reset
     188      gosub reset
     189      goto mainloop
     190   elseif mode==0 then
     191      print "Control not in manual mode!"
     192      gosub reset 
     193      SET PRGPAR 1
     194      exit
    183195   elseif fuse==0 then
    184196      print "Motor-Power Fuse not OK!"
    185       goto reset
     197      gosub reset
     198      goto mainloop
    186199   elseif vltg==0 then
    187200      print "Overvoltage control broken!"
    188       goto reset
     201      gosub reset
     202      goto mainloop
    189203   elseif emcy==0 then
    190204      print "Please release Emergency Stop!"
    191       goto reset
    192    elseif mode==0 then
    193       print "Control not in manual mode!"
    194       goto reset
     205      gosub reset
     206      goto mainloop
    195207   elseif (ready==1) and (RF==0) then
    196208      print "DKC powered, RF=0... setting RF=AH=1!"
    … …  
    210222      waitt 100
    211223
    212       if (brake==0) then
     224      if (brake==0 and get cannr==3) then
    213225         out (kIoModule+1) 1
    214226         brake = 1
    … …  
    239251   backward = in 3       
    240252                           
    241    if     (forward==1) and (backward==0) then
    242       cvel  manvel             
    243    elseif (forward==0) and (backward==1) then
     253   if     (forward==1) and (backward==0) and (velo!=manvel) then
     254      cvel  manvel                                         
     255      velo = manvel
     256   elseif (forward==0) and (backward==1) and (velo!=-manvel) then
    244257      cvel -manvel
    245    else
    246       cvel 0       
     258      velo = -manvel
     259   elseif (forward==backward) and (velo!=0) then
     260      cvel 0
     261      velo = 0       
    247262   endif             
    248263goto mainloop       
    249264       
    250265SUBMAINPROG
     266   subprog reset
     267      out 1 0
     268      out 2 0   
     269      RF = 0
     270      motor off   
     271      velo = 0
     272      waitt 1000
     273   
     274      if (brake==1 and get cannr==3) then         
     275         waitt 3000          /* wait 3s for DKC to stop the motor */
     276         out (kIoModule+1) 0 /* brake the brake                   */
     277         waitt 1000
     278      endif   
     279   return
     280     
    251281   subprog suberror
    252282      out 1 0
    … …  
    254284      RF = 0
    255285      waitt 100
     286      velo = 0
    256287     
    257       if (brake==1) then
     288      if (brake==1 and get cannr==3) then
    258289         waitt 5000
    259290         out (kIoModule+1) 0
Note: See TracChangeset for help on using the changeset viewer.