This is the firmware for the LaOS - Laser Open Source project. You can use it to drive a laser cutter. For hardware and more information, look at our wiki: http://wiki.laoslaser.org
Dependencies: EthernetNetIf mbed
LaosMotion/LaosMotion.cpp
- Committer:
- fablabtruck
- Date:
- 2012-06-08
- Revision:
- 0:3852426a5068
File content as of revision 0:3852426a5068:
/** * 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 // 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); DigitalOut estep(p29); // NOK: CAN, (TODO) DigitalOut edir(p30); // NOK: CAN, (TODO) // laser PwmOut pwm(p22); // O1: PWM (Yellow) DigitalOut laser_enable(p21); // O2: enable laser DigitalOut o3(p6); // 03: NC DigitalOut laser(p5); // O4: (p5) LaserON (White) // Analog in/out (cover sensor) + NC DigitalIn cover(p19); // globals int step=0, command=0; int mark_speed = 100; // 100 [mm/sec] // 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() { #if DO_MOTION_TEST tActionRequest act[2]; int i=0; Timer t; #endif pwm.period(1.0 / cfg->pwmfreq); pwm = cfg->pwmmin/100.0; mark_speed = cfg->speed; //start.mode(PullUp); xhome.mode(PullUp); yhome.mode(PullUp); isHome = false; plan_init(); st_init(); reset(); mark_speed = cfg->speed; 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() { step = command = xstep = xdir = ystep = ydir = zstep = zdir = 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) { 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\n", x, y); } /** *** MoveTo() width specific speed (%) **/ void LaosMotion::moveTo(int x, int y, int z, int speed) { 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\n", x, y); } /** *** write() *** Write command and parameters to motion controller **/ void LaosMotion::write(int i) { static int x,y,z,power=10000; //if ( plan_queue_empty() ) //printf("Empty\n"); 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.param = power; action.ActionType = (command ? AT_LASER : AT_MOVE); if ( bitmap_enable && action.ActionType == AT_LASER) { action.ActionType = AT_BITMAP; bitmap_enable = 0; } action.target.feed_rate = 60.0 * (command ? mark_speed : cfg->speed ); plan_buffer_line(&action); break; } break; case 2: // move z switch(step) { case 1: z = i; step = 0; 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; } 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; break; case 101: power = val; printf("power: %d\n", power); step=0; 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 // copy data { if ( step-2 == bitmap_size ) { step = 0; printf("Bitmap: received %d dwords\n\r", bitmap_size); } bitmap[ (step-3) % BITMAP_SIZE ] = i; } 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) { int i=0; printf("Homing %d,%d, %d with speed %d\n", x, y, z, cfg->homespeed); xdir = cfg->xhomedir; ydir = cfg->yhomedir; zdir = cfg->zhomedir; led1 = 0; isHome = false; printf("Home Z...\n\r"); if (cfg->autozhome) { while ((zmin ^ cfg->zpol) && (zmax ^ cfg->zpol)) { zstep = 0; wait(cfg->homespeed/1E6); zstep = 1; wait(cfg->homespeed/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; } } }