|
/**
|
|
* LaosMotion.cpp
|
|
* Motion Controll functions for Laos system
|
|
*
|
|
* Copyright (c) 2011 Peter Brier
|
|
*
|
|
* This file is part of the LaOS project (see: http://laoslaser.org)
|
|
*spe
|
|
* 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 "global.h"
|
|
#include "LaosMotion.h"
|
|
#include "planner.h"
|
|
#include "stepper.h"
|
|
#include "pins.h"
|
|
|
|
// #define DO_MOTION_TEST 1
|
|
|
|
// globals
|
|
unsigned int step=0;
|
|
int command=0;
|
|
int mark_speed = 100; // 100 [mm/sec]
|
|
int bitmap_speed = 100; // 100 [mm/sec]
|
|
int power = 10000 ;
|
|
// next planner action to enqueue
|
|
tActionRequest action;
|
|
|
|
// position offsets
|
|
static int ofsx=0, ofsy=0, ofsz=0;
|
|
|
|
// Command interpreter
|
|
int param=0, val=0;
|
|
|
|
// Bitmap buffer
|
|
#define BITMAP_PIXELS (8192)
|
|
#define BITMAP_SIZE (BITMAP_PIXELS/32)
|
|
unsigned long bitmap[BITMAP_SIZE];
|
|
unsigned long bitmap_width=0; // nr of pixels
|
|
unsigned long bitmap_size=0; // nr of bytes
|
|
unsigned char bitmap_bpp=1, bitmap_enable=0;
|
|
|
|
/**
|
|
*** LaosMotion() Constructor
|
|
*** Make new motion object
|
|
**/
|
|
LaosMotion::LaosMotion()
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
#if DO_MOTION_TEST
|
|
tActionRequest act[2];
|
|
int i=0;
|
|
Timer t;
|
|
#endif
|
|
pwm.period(1.0 / cfg->pwmfreq);
|
|
pwm = cfg->pwmmin/100.0;
|
|
if ( laser == NULL ) laser = new DigitalOut(LASER_PIN);
|
|
*laser = LASEROFF;
|
|
|
|
mark_speed = cfg->speed;
|
|
//start.mode(PullUp);
|
|
xhome.mode(PullUp);
|
|
yhome.mode(PullUp);
|
|
isHome = false;
|
|
plan_init();
|
|
st_init();
|
|
reset();
|
|
mark_speed = cfg->speed;
|
|
bitmap_speed = cfg->xspeed;
|
|
action.param = 0;
|
|
action.target.x = action.target.y = action.target.z = action.target.e =0;
|
|
action.target.feed_rate = 60*mark_speed;
|
|
|
|
#if DO_MOTION_TEST
|
|
t.start();
|
|
act[0].ActionType = act[1].ActionType = AT_MOVE;
|
|
act[0].target.feed_rate = 60 * 100;
|
|
act[1].target.feed_rate = 60 * 200;
|
|
act[0].target.x = act[0].target.y = act[0].target.z = act[0].target.e = 0;
|
|
act[1].target.x = act[1].target.y = act[1].target.z = act[1].target.e = 100;
|
|
act[1].target.y = 200;
|
|
while(1)
|
|
{
|
|
while( plan_queue_full() ) led3 = !led3;
|
|
led1 = 1;
|
|
i++;
|
|
if ( i )
|
|
printf("%d PING...\n", t.read_ms());
|
|
else
|
|
printf("%d PONG...\n", t.read_ms());
|
|
if ( i > 1 || i<0) i = 0;
|
|
plan_buffer_line (&act[i]);
|
|
led1 = 0;
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
***Destructor
|
|
**/
|
|
LaosMotion::~LaosMotion()
|
|
{
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
*** reset()
|
|
*** reset the state
|
|
**/
|
|
void LaosMotion::reset()
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
#ifdef READ_FILE_DEBUG
|
|
printf("LaosMotion::reset()\n");
|
|
#endif
|
|
xstep = xdir = ystep = ydir = zstep = zdir = step = command = 0;
|
|
ofsx = ofsy = ofsz = 0;
|
|
*laser = LASEROFF;
|
|
enable = cfg->enable;
|
|
cover.mode(PullUp);
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
*** ready()
|
|
*** ready to receive new commands
|
|
**/
|
|
int LaosMotion::ready()
|
|
{
|
|
return !plan_queue_full();
|
|
}
|
|
|
|
|
|
/**
|
|
*** queue()
|
|
*** return nr of items in the queue (0 is empty)
|
|
**/
|
|
int LaosMotion::queue()
|
|
{
|
|
return plan_queue_items();
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
*** MoveTo()
|
|
**/
|
|
void LaosMotion::moveTo(int x, int y, int z)
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
action.target.x = ofsx + x/1000.0;
|
|
action.target.y = ofsy + y/1000.0;
|
|
action.target.z = ofsz + z/1000.0;
|
|
action.ActionType = AT_MOVE;
|
|
action.target.feed_rate = 60.0 * cfg->speed;
|
|
plan_buffer_line(&action);
|
|
//printf("To buffer: %d, %d, %d\n", x, y, z);
|
|
}
|
|
|
|
/**
|
|
*** MoveTo() width specific speed (%)
|
|
**/
|
|
void LaosMotion::moveTo(int x, int y, int z, int speed)
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
action.target.x = ofsx + x/1000.0;
|
|
action.target.y = ofsy + y/1000.0;
|
|
action.target.z = ofsz + z/1000.0;
|
|
action.ActionType = AT_MOVE;
|
|
action.target.feed_rate = (speed * 60.0 * cfg->speed) / 100;
|
|
plan_buffer_line(&action);
|
|
//printf("To buffer: %d, %d, %d, %d\n", x, y,z,speed);
|
|
}
|
|
|
|
/**
|
|
*** write()
|
|
*** Write command and parameters to motion controller
|
|
*** Parse all the integers found in the simplecode file per integer
|
|
**/
|
|
void LaosMotion::write(int i)
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
static int x=0,y=0,z=0;
|
|
//if ( plan_queue_empty() )
|
|
//printf("Empty\n");
|
|
|
|
|
|
#ifdef READ_FILE_DEBUG_VERBOSE
|
|
printf(">%i (command: %i, step: %i)\n",i,command,step);
|
|
#endif
|
|
|
|
if ( step == 0 )
|
|
{
|
|
command = i;
|
|
step++;
|
|
}
|
|
else
|
|
{
|
|
switch( command )
|
|
{
|
|
case 0: // move x,y (laser off)
|
|
case 1: // line x,y (laser on)
|
|
switch ( step )
|
|
{
|
|
case 1:
|
|
action.target.x = i/1000.0;
|
|
break;
|
|
case 2:
|
|
action.target.y = i/1000.0;;
|
|
step=0;
|
|
action.target.z = 0;
|
|
action.param = power;
|
|
action.ActionType = (command ? AT_LASER : AT_MOVE);
|
|
if ( bitmap_enable && (action.ActionType == AT_LASER))
|
|
{
|
|
action.ActionType = AT_BITMAP;
|
|
bitmap_enable = 0;
|
|
}
|
|
switch ( action.ActionType )
|
|
{
|
|
case AT_MOVE: action.target.feed_rate = 60 * cfg->speed; break;
|
|
case AT_LASER: action.target.feed_rate = 60 * mark_speed; break;
|
|
case AT_BITMAP: action.target.feed_rate = 60 * bitmap_speed; break;
|
|
case AT_MOVE_ENDSTOP: break;
|
|
case AT_WAIT: break;
|
|
}
|
|
|
|
if ( action.ActionType == AT_BITMAP )
|
|
{
|
|
while ( queue() );// printf("-"); // wait for queue to empty
|
|
plan_set_accel(cfg->xaccel);
|
|
plan_buffer_line(&action);
|
|
while ( queue() ); // printf("*"); // wait for queue to empty
|
|
plan_set_accel(cfg->accel);
|
|
}
|
|
else
|
|
plan_buffer_line(&action);
|
|
|
|
break;
|
|
}
|
|
break;
|
|
case 2: // move z
|
|
switch(step)
|
|
{
|
|
case 1:
|
|
step = 0;
|
|
z = action.target.z;
|
|
action.param = power;
|
|
action.ActionType = AT_MOVE;
|
|
action.target.feed_rate = 60.0 * cfg->speed;
|
|
plan_buffer_line(&action);
|
|
break;
|
|
}
|
|
break;
|
|
case 4: // set x,y,z (absolute)
|
|
switch ( step )
|
|
{
|
|
case 1:
|
|
x = i;
|
|
break;
|
|
case 2:
|
|
y = i;
|
|
break;
|
|
case 3:
|
|
z = i;
|
|
setPosition(x,y,z);
|
|
step=0;
|
|
break;
|
|
}
|
|
break;
|
|
case 5: // nop
|
|
step = 0;
|
|
break;
|
|
case 7: // set index,value
|
|
switch ( step )
|
|
{
|
|
case 1:
|
|
param = i;
|
|
break;
|
|
case 2:
|
|
val = i;
|
|
step = 0;
|
|
switch( param )
|
|
{
|
|
case 100:
|
|
if ( val < 1 ) val = 1;
|
|
if ( val > 9999 ) val = 10000;
|
|
mark_speed = val * cfg->speed / 10000;
|
|
bitmap_speed = val * cfg->xspeed / 10000;
|
|
#ifdef READ_FILE_DEBUG
|
|
printf("> speed: %i\n",mark_speed);
|
|
#endif
|
|
break;
|
|
case 101:
|
|
power = val;
|
|
#ifdef READ_FILE_DEBUG
|
|
printf("> power: %i\n",power);
|
|
#endif
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
break;
|
|
case 9: // Store bitmap mark data format: 9 <bpp> <width> <data-0> <data-1> ... <data-n>
|
|
if ( step == 1 )
|
|
{
|
|
bitmap_bpp = i;
|
|
}
|
|
else if ( step == 2 )
|
|
{
|
|
// if ( queue() ) printf("Queue not empty... wait...\n\r");
|
|
while ( queue() );// printf("+"); // wait for queue to empty
|
|
bitmap_width = i;
|
|
bitmap_enable = 1;
|
|
bitmap_size = (bitmap_bpp * bitmap_width) / 32;
|
|
if ( (bitmap_bpp * bitmap_width) % 32 ) // padd to next 32-bit
|
|
bitmap_size++;
|
|
// printf("\n\rBitmap: read %d dwords\n\r", bitmap_size);
|
|
|
|
}
|
|
else if ( step > 2 )// copy data
|
|
{
|
|
bitmap[ (step-3) % BITMAP_SIZE ] = i;
|
|
// printf("[%ld] = %ld\n", (step-3) % BITMAP_SIZE, i);
|
|
if ( step-2 == bitmap_size ) // last dword received
|
|
{
|
|
bitmap[ (step-2) % BITMAP_SIZE ] = 0;
|
|
step = 0;
|
|
// printf("Bitmap: received %d dwords\n\r", bitmap_size);
|
|
}
|
|
}
|
|
break;
|
|
default: // I do not understand: stop motion
|
|
step = 0;
|
|
break;
|
|
}
|
|
if ( step )
|
|
step++;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
*** Return true if start button is pressed
|
|
**/
|
|
bool LaosMotion::isStart()
|
|
{
|
|
return cover;
|
|
}
|
|
|
|
|
|
/**
|
|
*** Hard set the absolute position
|
|
*** Warning: only call when the motion is not busy!
|
|
**/
|
|
void LaosMotion::setPosition(int x, int y, int z)
|
|
{
|
|
plan_set_current_position_xyz(x/1000.0,y/1000.0,z/1000.0);
|
|
ofsx = ofsy = ofsz = 0;
|
|
}
|
|
|
|
/**
|
|
*** get the absolute position
|
|
**/
|
|
void LaosMotion::getPosition(int *x, int *y, int *z)
|
|
{
|
|
float xx,yy,zz;
|
|
plan_get_current_position_xyz(&xx, &yy, &zz);
|
|
*x = xx * 1000;
|
|
*y = yy * 1000;
|
|
*z = zz * 1000;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
*** set the origin to this absolute position
|
|
*** set to (0,0,0) to reset the orgin back to its original position.
|
|
*** Note: Make sure you only call this at stand-still (motion queue is empty), otherwise strange things may happen
|
|
**/
|
|
void LaosMotion::setOrigin(int x, int y, int z)
|
|
{
|
|
ofsx = x;
|
|
ofsy = y;
|
|
ofsz = z;
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
*** Home the axis, stop when both home switches are pressed
|
|
**/
|
|
void LaosMotion::home(int x, int y, int z)
|
|
{
|
|
extern GlobalConfig *cfg;
|
|
int i=0;
|
|
printf("Homing %d,%d, with speed %d\n", x, y, cfg->homespeed);
|
|
xdir = cfg->xhomedir;
|
|
ydir = cfg->yhomedir;
|
|
zdir = cfg->zhomedir;
|
|
led1 = 0;
|
|
isHome = false;
|
|
printf("Home Z...\n\r");
|
|
if (cfg->autozhome) {
|
|
printf("Homing %d with speed %d\n", z, cfg->zhomespeed);
|
|
while ((zmin ^ cfg->zpol) && (zmax ^ cfg->zpol)) {
|
|
zstep = 0;
|
|
wait(cfg->zhomespeed/1E6);
|
|
zstep = 1;
|
|
wait(cfg->zhomespeed/1E6);
|
|
}
|
|
}
|
|
printf("Home XY...\n\r");
|
|
while ( 1 )
|
|
{
|
|
xstep = ystep = 0;
|
|
wait(cfg->homespeed/1E6);
|
|
xstep = xhome ^ cfg->xpol;
|
|
ystep = yhome ^ cfg->ypol;
|
|
wait(cfg->homespeed/1E6);
|
|
|
|
led2 = !xhome;
|
|
led3 = !yhome;
|
|
led4 = ((i++) & 0x10000);
|
|
if ( !(xhome ^ cfg->xpol) && !(yhome ^ cfg->ypol) )
|
|
{
|
|
setPosition(x,y,z);
|
|
moveTo(x,y,z);
|
|
isHome = true;
|
|
printf("Home done.\n\r");
|
|
return;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|