Diff: LaosMotion/LaosMotion.cpp

Revision:

0:18ead85c200b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LaosMotion/LaosMotion.cpp	Thu Aug 11 09:33:15 2011 +0000
@@ -0,0 +1,285 @@
+/**
+ * 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;
+}
+
+
+
+