LaosMotion.cpp

00001 /**
00002  * LaosMotion.cpp
00003  * Motion Controll functions for Laos system
00004  *
00005  * Copyright (c) 2011 Peter Brier
00006  *
00007  *   This file is part of the LaOS project (see: http://laoslaser.org)
00008  *spe
00009  *   LaOS is free software: you can redistribute it and/or modify
00010  *   it under the terms of the GNU General Public License as published by
00011  *   the Free Software Foundation, either version 3 of the License, or
00012  *   (at your option) any later version.
00013  *
00014  *   LaOS is distributed in the hope that it will be useful,
00015  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *   GNU General Public License for more details.
00018  *
00019  *   You should have received a copy of the GNU General Public License
00020  *   along with LaOS.  If not, see <http://www.gnu.org/licenses/>.
00021  *
00022  * 
00023  *
00024  */
00025 #include "global.h"
00026 #include "LaosMotion.h"
00027 #include  "planner.h"
00028 #include  "stepper.h"
00029 #include  "pins.h"
00030 
00031 // #define DO_MOTION_TEST 1
00032 
00033 // status leds
00034 extern DigitalOut led1,led2,led3,led4;
00035 
00036 // Inputs;
00037 DigitalIn xhome(p8);
00038 DigitalIn yhome(p17);
00039 DigitalIn zmin(p15);
00040 DigitalIn zmax(p16);
00041 
00042 // motors
00043 DigitalOut enable(p7);
00044 DigitalOut xdir(p23);
00045 DigitalOut xstep(p24);
00046 DigitalOut ydir(p25);
00047 DigitalOut ystep(p26);
00048 DigitalOut zdir(p27);
00049 DigitalOut zstep(p28);
00050 DigitalOut estep(p29); // NOK: CAN, (TODO)
00051 DigitalOut edir(p30);  // NOK: CAN, (TODO)
00052 
00053 
00054 // laser
00055 PwmOut pwm(p22);                // O1: PWM (Yellow)
00056 DigitalOut laser_enable(p21);   // O2: enable laser
00057 DigitalOut o3(p6);              // 03: NC
00058 DigitalOut laser(p5);           // O4: (p5) LaserON (White)
00059 
00060 // Analog in/out (cover sensor) + NC
00061 DigitalIn cover(p19);
00062 
00063   
00064 // globals
00065 int step=0, command=0;
00066 int mark_speed = 100; // 100 [mm/sec] 
00067 
00068 // next planner action to enqueue
00069 tActionRequest  action; 
00070 
00071 // position offsets
00072 static int ofsx=0, ofsy=0, ofsz=0;
00073 
00074 // Command interpreter
00075 int param=0, val=0;
00076 
00077 // Bitmap buffer
00078 #define BITMAP_PIXELS  (8192)
00079 #define BITMAP_SIZE (BITMAP_PIXELS/32)
00080 unsigned long bitmap[BITMAP_SIZE];
00081 unsigned long bitmap_width=0; // nr of pixels
00082 unsigned long bitmap_size=0; // nr of bytes 
00083 unsigned char bitmap_bpp=1, bitmap_enable=0;
00084 
00085 /**
00086 *** LaosMotion() Constructor
00087 *** Make new motion object
00088 **/
00089 LaosMotion::LaosMotion()
00090 {
00091 #if DO_MOTION_TEST
00092   tActionRequest act[2];
00093   int i=0;
00094   Timer t;
00095 #endif
00096   pwm.period(1.0 / cfg->pwmfreq);
00097   pwm = cfg->pwmmin/100.0;
00098 
00099   mark_speed = cfg->speed;
00100   //start.mode(PullUp);
00101   xhome.mode(PullUp);
00102   yhome.mode(PullUp);
00103   isHome = false;
00104   plan_init();
00105   st_init();  
00106   reset();
00107   mark_speed = cfg->speed;
00108   action.param = 0;
00109   action.target.x = action.target.y = action.target.z = action.target.e =0;
00110   action.target.feed_rate = 60*mark_speed;
00111 
00112 #if DO_MOTION_TEST
00113   t.start();
00114   act[0].ActionType = act[1].ActionType =  AT_MOVE;
00115   act[0].target.feed_rate = 60 * 100;
00116   act[1].target.feed_rate = 60 * 200;
00117   act[0].target.x = act[0].target.y = act[0].target.z = act[0].target.e = 0;
00118   act[1].target.x = act[1].target.y = act[1].target.z = act[1].target.e = 100;
00119   act[1].target.y = 200;
00120   while(1)
00121   {
00122     while( plan_queue_full() ) led3 = !led3;
00123     led1 = 1;
00124     i++;
00125     if ( i )
00126       printf("%d PING...\n", t.read_ms());
00127     else
00128       printf("%d PONG...\n", t.read_ms());
00129     if ( i > 1 || i<0) i = 0;
00130     plan_buffer_line (&act[i]);
00131     led1 = 0;
00132   }
00133 #endif
00134   
00135 }
00136 
00137 
00138 /**
00139 ***Destructor
00140 **/
00141 LaosMotion::~LaosMotion()
00142 {
00143 
00144 }
00145 
00146 
00147 /**
00148 *** reset()
00149 *** reset the state
00150 **/
00151 void LaosMotion::reset()
00152 {
00153   step = command = xstep = xdir = ystep = ydir = zstep = zdir = 0;
00154   ofsx = ofsy = ofsz = 0;
00155   laser = LASEROFF;
00156   enable = cfg->enable;
00157   cover.mode(PullUp);
00158 }
00159 
00160 
00161 
00162 /**
00163 *** ready()
00164 *** ready to receive new commands 
00165 **/
00166 int LaosMotion::ready()
00167 {
00168   return !plan_queue_full();
00169 }
00170 
00171 
00172 /**
00173 *** queue()
00174 *** return nr of items in the queue (0 is empty)
00175 **/
00176 int LaosMotion::queue()
00177 {
00178   return plan_queue_items();
00179 }
00180 
00181 
00182 
00183 
00184 /** 
00185 *** MoveTo()
00186 **/
00187 void LaosMotion::moveTo(int x, int y, int z)
00188 {
00189    action.target.x = ofsx + x/1000.0;
00190    action.target.y = ofsy + y/1000.0;
00191    action.target.z = ofsz + z/1000.0;
00192    action.ActionType = AT_MOVE;
00193    action.target.feed_rate =  60.0 * cfg->speed;
00194    plan_buffer_line(&action);
00195   // printf("To buffer: %d, %d\n", x, y);
00196 }
00197 
00198 /** 
00199 *** MoveTo() width specific speed (%)
00200 **/
00201 void LaosMotion::moveTo(int x, int y, int z, int speed)
00202 {
00203    action.target.x = ofsx + x/1000.0;
00204    action.target.y = ofsy + y/1000.0;
00205    action.target.z = ofsz + z/1000.0;
00206    action.ActionType = AT_MOVE;
00207    action.target.feed_rate =  (speed * 60.0 * cfg->speed) / 100;
00208    plan_buffer_line(&action);
00209    //printf("To buffer: %d, %d\n", x, y);
00210 }
00211 
00212 /**
00213 *** write()
00214 *** Write command and parameters to motion controller 
00215 **/
00216 void LaosMotion::write(int i)
00217 {
00218   static int x,y,z,power=10000;
00219   //if (  plan_queue_empty() ) 
00220   //printf("Empty\n");
00221   if ( step == 0 )
00222   {
00223     command = i;
00224     step++;
00225   }
00226   else
00227   { 
00228      switch( command )
00229      {
00230           case 0: // move x,y (laser off) 
00231           case 1: // line x,y (laser on)
00232             switch ( step )
00233             {
00234               case 1:
00235                 action.target.x = i/1000.0; 
00236                 break;
00237               case 2: 
00238                 action.target.y = i/1000.0;;
00239                 step=0;
00240                 action.param = power;
00241                 action.ActionType =  (command ? AT_LASER : AT_MOVE);
00242                 if ( bitmap_enable && action.ActionType == AT_LASER) 
00243                 { 
00244                   action.ActionType = AT_BITMAP;                 
00245                   bitmap_enable = 0;
00246                 }
00247                 action.target.feed_rate =  60.0 * (command ? mark_speed : cfg->speed );
00248                 plan_buffer_line(&action);
00249                 break;
00250             } 
00251             break;
00252           case 2: // move z 
00253             switch(step)
00254             {
00255               case 1: 
00256                 z = i; 
00257                 step = 0; 
00258                 break;
00259             }
00260          case 4: // set x,y,z (absolute)
00261            switch ( step )
00262             {
00263               case 1: 
00264                 x = i; 
00265                 break;
00266               case 2: 
00267                 y = i; 
00268                 break;
00269               case 3: 
00270                 z = i;
00271                 setPosition(x,y,z); 
00272                 step=0; 
00273                 break;
00274             }
00275          case 5: // nop
00276            step = 0;
00277            break;
00278          case 7: // set index,value
00279             switch ( step )
00280             {
00281               case 1: 
00282                 param = i; 
00283                 break;
00284               case 2: val = i; step = 0; 
00285               switch( param )
00286               {
00287                 case 100: 
00288                   if ( val < 1 ) val = 1;
00289                   if ( val > 9999 ) val = 10000; 
00290                   mark_speed = val * cfg->speed / 10000; 
00291                   break;  
00292                 case 101: 
00293                   power = val;
00294                   printf("power: %d\n", power); 
00295                   step=0; 
00296                   break;                  
00297               }
00298               break;
00299             }
00300          case 9: // Store bitmap mark data format: 9 <bpp> <width> <data-0> <data-1> ... <data-n>
00301             if ( step == 1 ) 
00302             {
00303               bitmap_bpp = i;
00304             }        
00305             else if ( step == 2 )
00306             {
00307               if ( queue() ) printf("Queue not empty... wait...\n\r");
00308               while ( queue() );// printf("+"); // wait for queue to empty 
00309               bitmap_width = i;
00310               bitmap_enable = 1;
00311               bitmap_size = (bitmap_bpp * bitmap_width) / 32;
00312               if  ( (bitmap_bpp * bitmap_width) % 32 )  // padd to next 32-bit 
00313                 bitmap_size++; 
00314               printf("\n\rBitmap: read %d dwords\n\r", bitmap_size);
00315 
00316             }
00317             else // copy data
00318             {
00319               if ( step-2 == bitmap_size )
00320               {
00321                 step = 0;
00322                 printf("Bitmap: received %d dwords\n\r", bitmap_size);
00323               }
00324               bitmap[ (step-3) % BITMAP_SIZE ] = i;
00325             }
00326            break; 
00327          default: // I do not understand: stop motion
00328             step = 0;
00329           break;
00330     }
00331     if ( step ) step++;
00332   } 
00333 }
00334 
00335 
00336 /**
00337 *** Return true if start button is pressed
00338 **/
00339 bool LaosMotion::isStart()
00340 {
00341   return cover;
00342 }
00343 
00344 
00345 /**
00346 *** Hard set the absolute position
00347 *** Warning: only call when the motion is not busy!
00348 **/
00349 void LaosMotion::setPosition(int x, int y, int z)
00350 {
00351   plan_set_current_position_xyz(x/1000.0,y/1000.0,z/1000.0);
00352   ofsx = ofsy = ofsz = 0;
00353 }
00354 
00355 /**
00356 *** get the absolute position
00357 **/
00358 void LaosMotion::getPosition(int *x, int *y, int *z)
00359 {
00360   float xx,yy,zz;
00361   plan_get_current_position_xyz(&xx, &yy, &zz);
00362   *x = xx * 1000;
00363   *y = yy * 1000;
00364   *z = zz * 1000;
00365 }
00366 
00367 
00368 
00369 /**
00370 *** set the origin to this absolute position
00371 *** set to (0,0,0) to reset the orgin back to its original position. 
00372 *** Note: Make sure you only call this at stand-still (motion queue is empty), otherwise strange things may happen
00373 **/
00374 void LaosMotion::setOrigin(int x, int y, int z)
00375 {
00376   ofsx = x; 
00377   ofsy = y;
00378   ofsz = z;
00379 }
00380 
00381 
00382 
00383 
00384 /**
00385 *** Home the axis, stop when both home switches are pressed
00386 **/
00387 void LaosMotion::home(int x, int y, int z)
00388 {
00389   int i=0;
00390   printf("Homing %d,%d, %d with speed %d\n", x, y, z, cfg->homespeed);
00391   xdir = cfg->xhomedir;
00392   ydir = cfg->yhomedir;
00393   zdir = cfg->zhomedir;
00394   led1 = 0;
00395   isHome = false;
00396   printf("Home Z...\n\r");
00397   if (cfg->autozhome) {
00398     while ((zmin ^ cfg->zpol) && (zmax ^ cfg->zpol)) {
00399         zstep = 0;
00400         wait(cfg->homespeed/1E6);
00401         zstep = 1;
00402         wait(cfg->homespeed/1E6);
00403     }
00404   }
00405   printf("Home XY...\n\r");
00406   while ( 1 )
00407   {
00408     xstep = ystep = 0;
00409     wait(cfg->homespeed/1E6);  
00410     xstep = xhome ^ cfg->xpol;
00411     ystep = yhome ^ cfg->ypol;
00412     wait(cfg->homespeed/1E6);
00413     
00414     led2 = !xhome;
00415     led3 = !yhome;
00416     led4 = ((i++) & 0x10000);
00417     if ( !(xhome ^ cfg->xpol) && !(yhome ^ cfg->ypol) ) 
00418     {
00419       setPosition(x,y,z);
00420       moveTo(x,y,z);
00421       isHome = true;
00422       printf("Home done.\n\r");
00423       return;
00424     }
00425   }
00426  
00427 }
00428 
00429 
00430 
00431