File content as of revision 0:18ead85c200b:

/**
 * LaosMotion.cpp
 * Motion Controll functions for Laos system
 *
 * Copyright (c) 2011 Peter Brier
 *
 *   This file is part of the LaOS project (see: http://wiki.protospace.nl/index.php/LaOS)
 *
 *   LaOS is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   LaOS is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with LaOS.  If not, see <http://www.gnu.org/licenses/>.
 *
 * 
 *
 */
#include "LaosMotion.h"

  
// status leds
extern DigitalOut led1,led2,led3,led4;

// Inputs;
DigitalIn xhome(p8);
DigitalIn yhome(p17);
DigitalIn zmin(p15);
DigitalIn zmax(p16);

// motors
DigitalOut enable(p7);
DigitalOut xdir(p23);
DigitalOut xstep(p24);
DigitalOut ydir(p25);
DigitalOut ystep(p26);
DigitalOut zdir(p27);
DigitalOut zstep(p28);

// laser

// DigitalOut laser(p22);       // O1: PWM (Yellow)
PwmOut pwm(p22);
DigitalOut laser_enable(p21);   // O2: enable laser
DigitalOut o3(p6);              // 03: NC
DigitalOut laser(p5);           // O4: LaserON (White)


// Analog in/out (cover sensor) + NC
DigitalIn cover(p19);

// globals
Ticker ticker;  // Timer
int ctr;

int step, prevcommand, command;
volatile int state, laseron;

// Besenham variables
int x0,y0,x1,y1,z0,z1;  // positions: 0=current, 1=new
int dx, sx; // step
int dy, sy;  // delta
int err, e2; // error


#define LASEROFF 1
#define LASERON 0

#define LOW_SPEED 400
#define HIGH_SPEED 90


/**
*** ticker: implement timer ticker, step if required
**/
void tick() 
{
  ctr++;
  // Control X,Y: line tracker
  switch ( state )
  {
    case 0: // idle (make sure laser is off)
      laser=LASEROFF;
      break;
    case 1: // init
      dx = abs(x1-x0);
      sx = x0<x1 ? 1 : -1;
      dy = abs(y1-y0);
      sy = y0<y1 ? 1 : -1; 
      err = (dx>dy ? dx : -dy)/2;
      xdir = sx > 0;
      ydir = sy > 0;
      if ( laseron )
        laser = LASERON;
      else
        laser = LASEROFF;
      state++;
      break;
    case 2: // moving
      if (x0==x1 && y0==y1) 
        state = 0;
      else
      {
        e2 = err;
        if (e2 >-dx) { err -= dy; x0 += sx; xstep = 1;}
        if (e2 < dy) { err += dx; y0 += sy; ystep = 1;}
      }
      break;
  }
  // Controll Z: tracking controller (independent of XY motion, not queued)
  if ( z1 != z0 ) 
  {
    zdir = z1 > z0;
    wait_us(5);
    zstep= 1;
    if ( z1 > z0 ) z0++;
    else z0--;
  }
  
  if ( xstep == 1 || ystep == 1 || zstep == 1 )
  {
    wait_us(5);
    xstep = ystep = zstep = 0;
  }
  
  // diagnostic info
  led1 = xdir;
  led2 = ydir;
  led3 = laseron;
  led4 = ctr & 4096;
} // ticker


/**
*** LaosMotion() Constructor
*** Make new motion object
**/
LaosMotion::LaosMotion()
{
  reset();
  laser=LASEROFF;
  pwm.period(0.0005);
  pwm = 0.5;
  //start.mode(PullUp);
  xhome.mode(PullUp);
  yhome.mode(PullUp);
  isHome = false;
  ticker.attach_us(&tick, HIGH_SPEED); // the address of the function to be attached (flip) and the interval (microseconds)
}

/**
***Destructor
**/
LaosMotion::~LaosMotion()
{

}

/**
*** reset()
*** reset the state
**/
void LaosMotion::reset()
{
  step = prevcommand = command = state = laseron = xstep = xdir = ystep = ydir = zstep = zdir = 0;
  laser=LASEROFF;
}



/**
*** ready()
*** ready to receive new commands 
**/
int LaosMotion::ready()
{
  return state == 0;
}

/**
*** write()
*** Write command to motion controller 
**/
void LaosMotion::write(int i)
{
 // printf("m:%d %d %d %d\n", state, step, i, command);
  if ( step == 0 )
  {
    prevcommand = command;
    command = i;
    step++;
  }
  else
  { 
     switch( command )
     {
          case 0: // move x,y (laser off)
          case 1: // line x,y (laser on)
            switch ( step )
            {
              case 1: x1 = i; break;
              case 2: 
                y1 = i;
                if ( prevcommand != command )
                 ticker.attach_us(&tick, (command ? LOW_SPEED : HIGH_SPEED)); // the address of the function to be attached (flip) and the interval (microseconds)
                laseron = (command == 1); state = 1; step = 0;
                break;
            } 
            break;
          case 2: // move z 
            switch(step)
            {
              case 1: z1 = i; step = 0; break;
            }
          case 3: // set t (1/v)
          switch(step)
          {
            case 1: ticker.attach_us(&tick, i); step=0; break;
          }
         case 4: // set x,y,z absolute
           switch ( step )
            {
              case 1: x0 = i; break;
              case 2: y0 = i; break;
              case 3: z0=z1 = i; step=0; break;
            }
         case 5: // nop
           step=0;
           break;
         default: // I do not understand: stop motion
            state = 0;
            step = 0;
          break;
    }
    if ( step ) step++;
  } 
}


/**
*** Return true if start button is pressed
**/
bool LaosMotion::isStart()
{
  return cover;
}


/**
*** Home the axis, stop when button is pressed
**/
void LaosMotion::home()
{
  int i=0;
  xdir = 0;
  ydir = 0;
  zdir = 0;
  led1 = 0;
  isHome = false;
  while ( 1 )
  {
    xstep = ystep = 0;
    wait(0.0001);
    xstep = xhome;
    ystep = yhome;
    wait(0.0001);
    
    led2 = !xhome;
    led3 = !yhome;
    led4 = ((i++) & 0x10000);
    if ( (!xhome) && (!yhome) ) return;
 //   if ( i > 2000 && !isStart()  ) return;  // debounce, and quit if key pressed again
  }
  isHome = true;
}