LaOS Laser / Mbed 2 deprecated laoslaser

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@0:3852426a5068, 2012-06-08 (annotated)

Committer:
fablabtruck
Date:
Fri Jun 08 09:26:40 2012 +0000
Revision:
0:3852426a5068
svn revision 379

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fablabtruck 0:3852426a5068 1 /**
fablabtruck 0:3852426a5068 2 * LaosMotion.cpp
fablabtruck 0:3852426a5068 3 * Motion Controll functions for Laos system
fablabtruck 0:3852426a5068 4 *
fablabtruck 0:3852426a5068 5 * Copyright (c) 2011 Peter Brier
fablabtruck 0:3852426a5068 6 *
fablabtruck 0:3852426a5068 7 * This file is part of the LaOS project (see: http://laoslaser.org)
fablabtruck 0:3852426a5068 8 *spe
fablabtruck 0:3852426a5068 9 * LaOS is free software: you can redistribute it and/or modify
fablabtruck 0:3852426a5068 10 * it under the terms of the GNU General Public License as published by
fablabtruck 0:3852426a5068 11 * the Free Software Foundation, either version 3 of the License, or
fablabtruck 0:3852426a5068 12 * (at your option) any later version.
fablabtruck 0:3852426a5068 13 *
fablabtruck 0:3852426a5068 14 * LaOS is distributed in the hope that it will be useful,
fablabtruck 0:3852426a5068 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
fablabtruck 0:3852426a5068 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
fablabtruck 0:3852426a5068 17 * GNU General Public License for more details.
fablabtruck 0:3852426a5068 18 *
fablabtruck 0:3852426a5068 19 * You should have received a copy of the GNU General Public License
fablabtruck 0:3852426a5068 20 * along with LaOS. If not, see <http://www.gnu.org/licenses/>.
fablabtruck 0:3852426a5068 21 *
fablabtruck 0:3852426a5068 22 *
fablabtruck 0:3852426a5068 23 *
fablabtruck 0:3852426a5068 24 */
fablabtruck 0:3852426a5068 25 #include "global.h"
fablabtruck 0:3852426a5068 26 #include "LaosMotion.h"
fablabtruck 0:3852426a5068 27 #include "planner.h"
fablabtruck 0:3852426a5068 28 #include "stepper.h"
fablabtruck 0:3852426a5068 29 #include "pins.h"
fablabtruck 0:3852426a5068 30
fablabtruck 0:3852426a5068 31 // #define DO_MOTION_TEST 1
fablabtruck 0:3852426a5068 32
fablabtruck 0:3852426a5068 33 // status leds
fablabtruck 0:3852426a5068 34 extern DigitalOut led1,led2,led3,led4;
fablabtruck 0:3852426a5068 35
fablabtruck 0:3852426a5068 36 // Inputs;
fablabtruck 0:3852426a5068 37 DigitalIn xhome(p8);
fablabtruck 0:3852426a5068 38 DigitalIn yhome(p17);
fablabtruck 0:3852426a5068 39 DigitalIn zmin(p15);
fablabtruck 0:3852426a5068 40 DigitalIn zmax(p16);
fablabtruck 0:3852426a5068 41
fablabtruck 0:3852426a5068 42 // motors
fablabtruck 0:3852426a5068 43 DigitalOut enable(p7);
fablabtruck 0:3852426a5068 44 DigitalOut xdir(p23);
fablabtruck 0:3852426a5068 45 DigitalOut xstep(p24);
fablabtruck 0:3852426a5068 46 DigitalOut ydir(p25);
fablabtruck 0:3852426a5068 47 DigitalOut ystep(p26);
fablabtruck 0:3852426a5068 48 DigitalOut zdir(p27);
fablabtruck 0:3852426a5068 49 DigitalOut zstep(p28);
fablabtruck 0:3852426a5068 50 DigitalOut estep(p29); // NOK: CAN, (TODO)
fablabtruck 0:3852426a5068 51 DigitalOut edir(p30); // NOK: CAN, (TODO)
fablabtruck 0:3852426a5068 52
fablabtruck 0:3852426a5068 53
fablabtruck 0:3852426a5068 54 // laser
fablabtruck 0:3852426a5068 55 PwmOut pwm(p22); // O1: PWM (Yellow)
fablabtruck 0:3852426a5068 56 DigitalOut laser_enable(p21); // O2: enable laser
fablabtruck 0:3852426a5068 57 DigitalOut o3(p6); // 03: NC
fablabtruck 0:3852426a5068 58 DigitalOut laser(p5); // O4: (p5) LaserON (White)
fablabtruck 0:3852426a5068 59
fablabtruck 0:3852426a5068 60 // Analog in/out (cover sensor) + NC
fablabtruck 0:3852426a5068 61 DigitalIn cover(p19);
fablabtruck 0:3852426a5068 62
fablabtruck 0:3852426a5068 63
fablabtruck 0:3852426a5068 64 // globals
fablabtruck 0:3852426a5068 65 int step=0, command=0;
fablabtruck 0:3852426a5068 66 int mark_speed = 100; // 100 [mm/sec]
fablabtruck 0:3852426a5068 67
fablabtruck 0:3852426a5068 68 // next planner action to enqueue
fablabtruck 0:3852426a5068 69 tActionRequest action;
fablabtruck 0:3852426a5068 70
fablabtruck 0:3852426a5068 71 // position offsets
fablabtruck 0:3852426a5068 72 static int ofsx=0, ofsy=0, ofsz=0;
fablabtruck 0:3852426a5068 73
fablabtruck 0:3852426a5068 74 // Command interpreter
fablabtruck 0:3852426a5068 75 int param=0, val=0;
fablabtruck 0:3852426a5068 76
fablabtruck 0:3852426a5068 77 // Bitmap buffer
fablabtruck 0:3852426a5068 78 #define BITMAP_PIXELS (8192)
fablabtruck 0:3852426a5068 79 #define BITMAP_SIZE (BITMAP_PIXELS/32)
fablabtruck 0:3852426a5068 80 unsigned long bitmap[BITMAP_SIZE];
fablabtruck 0:3852426a5068 81 unsigned long bitmap_width=0; // nr of pixels
fablabtruck 0:3852426a5068 82 unsigned long bitmap_size=0; // nr of bytes
fablabtruck 0:3852426a5068 83 unsigned char bitmap_bpp=1, bitmap_enable=0;
fablabtruck 0:3852426a5068 84
fablabtruck 0:3852426a5068 85 /**
fablabtruck 0:3852426a5068 86 *** LaosMotion() Constructor
fablabtruck 0:3852426a5068 87 *** Make new motion object
fablabtruck 0:3852426a5068 88 **/
fablabtruck 0:3852426a5068 89 LaosMotion::LaosMotion()
fablabtruck 0:3852426a5068 90 {
fablabtruck 0:3852426a5068 91 #if DO_MOTION_TEST
fablabtruck 0:3852426a5068 92 tActionRequest act[2];
fablabtruck 0:3852426a5068 93 int i=0;
fablabtruck 0:3852426a5068 94 Timer t;
fablabtruck 0:3852426a5068 95 #endif
fablabtruck 0:3852426a5068 96 pwm.period(1.0 / cfg->pwmfreq);
fablabtruck 0:3852426a5068 97 pwm = cfg->pwmmin/100.0;
fablabtruck 0:3852426a5068 98
fablabtruck 0:3852426a5068 99 mark_speed = cfg->speed;
fablabtruck 0:3852426a5068 100 //start.mode(PullUp);
fablabtruck 0:3852426a5068 101 xhome.mode(PullUp);
fablabtruck 0:3852426a5068 102 yhome.mode(PullUp);
fablabtruck 0:3852426a5068 103 isHome = false;
fablabtruck 0:3852426a5068 104 plan_init();
fablabtruck 0:3852426a5068 105 st_init();
fablabtruck 0:3852426a5068 106 reset();
fablabtruck 0:3852426a5068 107 mark_speed = cfg->speed;
fablabtruck 0:3852426a5068 108 action.param = 0;
fablabtruck 0:3852426a5068 109 action.target.x = action.target.y = action.target.z = action.target.e =0;
fablabtruck 0:3852426a5068 110 action.target.feed_rate = 60*mark_speed;
fablabtruck 0:3852426a5068 111
fablabtruck 0:3852426a5068 112 #if DO_MOTION_TEST
fablabtruck 0:3852426a5068 113 t.start();
fablabtruck 0:3852426a5068 114 act[0].ActionType = act[1].ActionType = AT_MOVE;
fablabtruck 0:3852426a5068 115 act[0].target.feed_rate = 60 * 100;
fablabtruck 0:3852426a5068 116 act[1].target.feed_rate = 60 * 200;
fablabtruck 0:3852426a5068 117 act[0].target.x = act[0].target.y = act[0].target.z = act[0].target.e = 0;
fablabtruck 0:3852426a5068 118 act[1].target.x = act[1].target.y = act[1].target.z = act[1].target.e = 100;
fablabtruck 0:3852426a5068 119 act[1].target.y = 200;
fablabtruck 0:3852426a5068 120 while(1)
fablabtruck 0:3852426a5068 121 {
fablabtruck 0:3852426a5068 122 while( plan_queue_full() ) led3 = !led3;
fablabtruck 0:3852426a5068 123 led1 = 1;
fablabtruck 0:3852426a5068 124 i++;
fablabtruck 0:3852426a5068 125 if ( i )
fablabtruck 0:3852426a5068 126 printf("%d PING...\n", t.read_ms());
fablabtruck 0:3852426a5068 127 else
fablabtruck 0:3852426a5068 128 printf("%d PONG...\n", t.read_ms());
fablabtruck 0:3852426a5068 129 if ( i > 1 || i<0) i = 0;
fablabtruck 0:3852426a5068 130 plan_buffer_line (&act[i]);
fablabtruck 0:3852426a5068 131 led1 = 0;
fablabtruck 0:3852426a5068 132 }
fablabtruck 0:3852426a5068 133 #endif
fablabtruck 0:3852426a5068 134
fablabtruck 0:3852426a5068 135 }
fablabtruck 0:3852426a5068 136
fablabtruck 0:3852426a5068 137
fablabtruck 0:3852426a5068 138 /**
fablabtruck 0:3852426a5068 139 ***Destructor
fablabtruck 0:3852426a5068 140 **/
fablabtruck 0:3852426a5068 141 LaosMotion::~LaosMotion()
fablabtruck 0:3852426a5068 142 {
fablabtruck 0:3852426a5068 143
fablabtruck 0:3852426a5068 144 }
fablabtruck 0:3852426a5068 145
fablabtruck 0:3852426a5068 146
fablabtruck 0:3852426a5068 147 /**
fablabtruck 0:3852426a5068 148 *** reset()
fablabtruck 0:3852426a5068 149 *** reset the state
fablabtruck 0:3852426a5068 150 **/
fablabtruck 0:3852426a5068 151 void LaosMotion::reset()
fablabtruck 0:3852426a5068 152 {
fablabtruck 0:3852426a5068 153 step = command = xstep = xdir = ystep = ydir = zstep = zdir = 0;
fablabtruck 0:3852426a5068 154 ofsx = ofsy = ofsz = 0;
fablabtruck 0:3852426a5068 155 laser = LASEROFF;
fablabtruck 0:3852426a5068 156 enable = cfg->enable;
fablabtruck 0:3852426a5068 157 cover.mode(PullUp);
fablabtruck 0:3852426a5068 158 }
fablabtruck 0:3852426a5068 159
fablabtruck 0:3852426a5068 160
fablabtruck 0:3852426a5068 161
fablabtruck 0:3852426a5068 162 /**
fablabtruck 0:3852426a5068 163 *** ready()
fablabtruck 0:3852426a5068 164 *** ready to receive new commands
fablabtruck 0:3852426a5068 165 **/
fablabtruck 0:3852426a5068 166 int LaosMotion::ready()
fablabtruck 0:3852426a5068 167 {
fablabtruck 0:3852426a5068 168 return !plan_queue_full();
fablabtruck 0:3852426a5068 169 }
fablabtruck 0:3852426a5068 170
fablabtruck 0:3852426a5068 171
fablabtruck 0:3852426a5068 172 /**
fablabtruck 0:3852426a5068 173 *** queue()
fablabtruck 0:3852426a5068 174 *** return nr of items in the queue (0 is empty)
fablabtruck 0:3852426a5068 175 **/
fablabtruck 0:3852426a5068 176 int LaosMotion::queue()
fablabtruck 0:3852426a5068 177 {
fablabtruck 0:3852426a5068 178 return plan_queue_items();
fablabtruck 0:3852426a5068 179 }
fablabtruck 0:3852426a5068 180
fablabtruck 0:3852426a5068 181
fablabtruck 0:3852426a5068 182
fablabtruck 0:3852426a5068 183
fablabtruck 0:3852426a5068 184 /**
fablabtruck 0:3852426a5068 185 *** MoveTo()
fablabtruck 0:3852426a5068 186 **/
fablabtruck 0:3852426a5068 187 void LaosMotion::moveTo(int x, int y, int z)
fablabtruck 0:3852426a5068 188 {
fablabtruck 0:3852426a5068 189 action.target.x = ofsx + x/1000.0;
fablabtruck 0:3852426a5068 190 action.target.y = ofsy + y/1000.0;
fablabtruck 0:3852426a5068 191 action.target.z = ofsz + z/1000.0;
fablabtruck 0:3852426a5068 192 action.ActionType = AT_MOVE;
fablabtruck 0:3852426a5068 193 action.target.feed_rate = 60.0 * cfg->speed;
fablabtruck 0:3852426a5068 194 plan_buffer_line(&action);
fablabtruck 0:3852426a5068 195 // printf("To buffer: %d, %d\n", x, y);
fablabtruck 0:3852426a5068 196 }
fablabtruck 0:3852426a5068 197
fablabtruck 0:3852426a5068 198 /**
fablabtruck 0:3852426a5068 199 *** MoveTo() width specific speed (%)
fablabtruck 0:3852426a5068 200 **/
fablabtruck 0:3852426a5068 201 void LaosMotion::moveTo(int x, int y, int z, int speed)
fablabtruck 0:3852426a5068 202 {
fablabtruck 0:3852426a5068 203 action.target.x = ofsx + x/1000.0;
fablabtruck 0:3852426a5068 204 action.target.y = ofsy + y/1000.0;
fablabtruck 0:3852426a5068 205 action.target.z = ofsz + z/1000.0;
fablabtruck 0:3852426a5068 206 action.ActionType = AT_MOVE;
fablabtruck 0:3852426a5068 207 action.target.feed_rate = (speed * 60.0 * cfg->speed) / 100;
fablabtruck 0:3852426a5068 208 plan_buffer_line(&action);
fablabtruck 0:3852426a5068 209 //printf("To buffer: %d, %d\n", x, y);
fablabtruck 0:3852426a5068 210 }
fablabtruck 0:3852426a5068 211
fablabtruck 0:3852426a5068 212 /**
fablabtruck 0:3852426a5068 213 *** write()
fablabtruck 0:3852426a5068 214 *** Write command and parameters to motion controller
fablabtruck 0:3852426a5068 215 **/
fablabtruck 0:3852426a5068 216 void LaosMotion::write(int i)
fablabtruck 0:3852426a5068 217 {
fablabtruck 0:3852426a5068 218 static int x,y,z,power=10000;
fablabtruck 0:3852426a5068 219 //if ( plan_queue_empty() )
fablabtruck 0:3852426a5068 220 //printf("Empty\n");
fablabtruck 0:3852426a5068 221 if ( step == 0 )
fablabtruck 0:3852426a5068 222 {
fablabtruck 0:3852426a5068 223 command = i;
fablabtruck 0:3852426a5068 224 step++;
fablabtruck 0:3852426a5068 225 }
fablabtruck 0:3852426a5068 226 else
fablabtruck 0:3852426a5068 227 {
fablabtruck 0:3852426a5068 228 switch( command )
fablabtruck 0:3852426a5068 229 {
fablabtruck 0:3852426a5068 230 case 0: // move x,y (laser off)
fablabtruck 0:3852426a5068 231 case 1: // line x,y (laser on)
fablabtruck 0:3852426a5068 232 switch ( step )
fablabtruck 0:3852426a5068 233 {
fablabtruck 0:3852426a5068 234 case 1:
fablabtruck 0:3852426a5068 235 action.target.x = i/1000.0;
fablabtruck 0:3852426a5068 236 break;
fablabtruck 0:3852426a5068 237 case 2:
fablabtruck 0:3852426a5068 238 action.target.y = i/1000.0;;
fablabtruck 0:3852426a5068 239 step=0;
fablabtruck 0:3852426a5068 240 action.param = power;
fablabtruck 0:3852426a5068 241 action.ActionType = (command ? AT_LASER : AT_MOVE);
fablabtruck 0:3852426a5068 242 if ( bitmap_enable && action.ActionType == AT_LASER)
fablabtruck 0:3852426a5068 243 {
fablabtruck 0:3852426a5068 244 action.ActionType = AT_BITMAP;
fablabtruck 0:3852426a5068 245 bitmap_enable = 0;
fablabtruck 0:3852426a5068 246 }
fablabtruck 0:3852426a5068 247 action.target.feed_rate = 60.0 * (command ? mark_speed : cfg->speed );
fablabtruck 0:3852426a5068 248 plan_buffer_line(&action);
fablabtruck 0:3852426a5068 249 break;
fablabtruck 0:3852426a5068 250 }
fablabtruck 0:3852426a5068 251 break;
fablabtruck 0:3852426a5068 252 case 2: // move z
fablabtruck 0:3852426a5068 253 switch(step)
fablabtruck 0:3852426a5068 254 {
fablabtruck 0:3852426a5068 255 case 1:
fablabtruck 0:3852426a5068 256 z = i;
fablabtruck 0:3852426a5068 257 step = 0;
fablabtruck 0:3852426a5068 258 break;
fablabtruck 0:3852426a5068 259 }
fablabtruck 0:3852426a5068 260 case 4: // set x,y,z (absolute)
fablabtruck 0:3852426a5068 261 switch ( step )
fablabtruck 0:3852426a5068 262 {
fablabtruck 0:3852426a5068 263 case 1:
fablabtruck 0:3852426a5068 264 x = i;
fablabtruck 0:3852426a5068 265 break;
fablabtruck 0:3852426a5068 266 case 2:
fablabtruck 0:3852426a5068 267 y = i;
fablabtruck 0:3852426a5068 268 break;
fablabtruck 0:3852426a5068 269 case 3:
fablabtruck 0:3852426a5068 270 z = i;
fablabtruck 0:3852426a5068 271 setPosition(x,y,z);
fablabtruck 0:3852426a5068 272 step=0;
fablabtruck 0:3852426a5068 273 break;
fablabtruck 0:3852426a5068 274 }
fablabtruck 0:3852426a5068 275 case 5: // nop
fablabtruck 0:3852426a5068 276 step = 0;
fablabtruck 0:3852426a5068 277 break;
fablabtruck 0:3852426a5068 278 case 7: // set index,value
fablabtruck 0:3852426a5068 279 switch ( step )
fablabtruck 0:3852426a5068 280 {
fablabtruck 0:3852426a5068 281 case 1:
fablabtruck 0:3852426a5068 282 param = i;
fablabtruck 0:3852426a5068 283 break;
fablabtruck 0:3852426a5068 284 case 2: val = i; step = 0;
fablabtruck 0:3852426a5068 285 switch( param )
fablabtruck 0:3852426a5068 286 {
fablabtruck 0:3852426a5068 287 case 100:
fablabtruck 0:3852426a5068 288 if ( val < 1 ) val = 1;
fablabtruck 0:3852426a5068 289 if ( val > 9999 ) val = 10000;
fablabtruck 0:3852426a5068 290 mark_speed = val * cfg->speed / 10000;
fablabtruck 0:3852426a5068 291 break;
fablabtruck 0:3852426a5068 292 case 101:
fablabtruck 0:3852426a5068 293 power = val;
fablabtruck 0:3852426a5068 294 printf("power: %d\n", power);
fablabtruck 0:3852426a5068 295 step=0;
fablabtruck 0:3852426a5068 296 break;
fablabtruck 0:3852426a5068 297 }
fablabtruck 0:3852426a5068 298 break;
fablabtruck 0:3852426a5068 299 }
fablabtruck 0:3852426a5068 300 case 9: // Store bitmap mark data format: 9 <bpp> <width> <data-0> <data-1> ... <data-n>
fablabtruck 0:3852426a5068 301 if ( step == 1 )
fablabtruck 0:3852426a5068 302 {
fablabtruck 0:3852426a5068 303 bitmap_bpp = i;
fablabtruck 0:3852426a5068 304 }
fablabtruck 0:3852426a5068 305 else if ( step == 2 )
fablabtruck 0:3852426a5068 306 {
fablabtruck 0:3852426a5068 307 if ( queue() ) printf("Queue not empty... wait...\n\r");
fablabtruck 0:3852426a5068 308 while ( queue() );// printf("+"); // wait for queue to empty
fablabtruck 0:3852426a5068 309 bitmap_width = i;
fablabtruck 0:3852426a5068 310 bitmap_enable = 1;
fablabtruck 0:3852426a5068 311 bitmap_size = (bitmap_bpp * bitmap_width) / 32;
fablabtruck 0:3852426a5068 312 if ( (bitmap_bpp * bitmap_width) % 32 ) // padd to next 32-bit
fablabtruck 0:3852426a5068 313 bitmap_size++;
fablabtruck 0:3852426a5068 314 printf("\n\rBitmap: read %d dwords\n\r", bitmap_size);
fablabtruck 0:3852426a5068 315
fablabtruck 0:3852426a5068 316 }
fablabtruck 0:3852426a5068 317 else // copy data
fablabtruck 0:3852426a5068 318 {
fablabtruck 0:3852426a5068 319 if ( step-2 == bitmap_size )
fablabtruck 0:3852426a5068 320 {
fablabtruck 0:3852426a5068 321 step = 0;
fablabtruck 0:3852426a5068 322 printf("Bitmap: received %d dwords\n\r", bitmap_size);
fablabtruck 0:3852426a5068 323 }
fablabtruck 0:3852426a5068 324 bitmap[ (step-3) % BITMAP_SIZE ] = i;
fablabtruck 0:3852426a5068 325 }
fablabtruck 0:3852426a5068 326 break;
fablabtruck 0:3852426a5068 327 default: // I do not understand: stop motion
fablabtruck 0:3852426a5068 328 step = 0;
fablabtruck 0:3852426a5068 329 break;
fablabtruck 0:3852426a5068 330 }
fablabtruck 0:3852426a5068 331 if ( step ) step++;
fablabtruck 0:3852426a5068 332 }
fablabtruck 0:3852426a5068 333 }
fablabtruck 0:3852426a5068 334
fablabtruck 0:3852426a5068 335
fablabtruck 0:3852426a5068 336 /**
fablabtruck 0:3852426a5068 337 *** Return true if start button is pressed
fablabtruck 0:3852426a5068 338 **/
fablabtruck 0:3852426a5068 339 bool LaosMotion::isStart()
fablabtruck 0:3852426a5068 340 {
fablabtruck 0:3852426a5068 341 return cover;
fablabtruck 0:3852426a5068 342 }
fablabtruck 0:3852426a5068 343
fablabtruck 0:3852426a5068 344
fablabtruck 0:3852426a5068 345 /**
fablabtruck 0:3852426a5068 346 *** Hard set the absolute position
fablabtruck 0:3852426a5068 347 *** Warning: only call when the motion is not busy!
fablabtruck 0:3852426a5068 348 **/
fablabtruck 0:3852426a5068 349 void LaosMotion::setPosition(int x, int y, int z)
fablabtruck 0:3852426a5068 350 {
fablabtruck 0:3852426a5068 351 plan_set_current_position_xyz(x/1000.0,y/1000.0,z/1000.0);
fablabtruck 0:3852426a5068 352 ofsx = ofsy = ofsz = 0;
fablabtruck 0:3852426a5068 353 }
fablabtruck 0:3852426a5068 354
fablabtruck 0:3852426a5068 355 /**
fablabtruck 0:3852426a5068 356 *** get the absolute position
fablabtruck 0:3852426a5068 357 **/
fablabtruck 0:3852426a5068 358 void LaosMotion::getPosition(int *x, int *y, int *z)
fablabtruck 0:3852426a5068 359 {
fablabtruck 0:3852426a5068 360 float xx,yy,zz;
fablabtruck 0:3852426a5068 361 plan_get_current_position_xyz(&xx, &yy, &zz);
fablabtruck 0:3852426a5068 362 *x = xx * 1000;
fablabtruck 0:3852426a5068 363 *y = yy * 1000;
fablabtruck 0:3852426a5068 364 *z = zz * 1000;
fablabtruck 0:3852426a5068 365 }
fablabtruck 0:3852426a5068 366
fablabtruck 0:3852426a5068 367
fablabtruck 0:3852426a5068 368
fablabtruck 0:3852426a5068 369 /**
fablabtruck 0:3852426a5068 370 *** set the origin to this absolute position
fablabtruck 0:3852426a5068 371 *** set to (0,0,0) to reset the orgin back to its original position.
fablabtruck 0:3852426a5068 372 *** Note: Make sure you only call this at stand-still (motion queue is empty), otherwise strange things may happen
fablabtruck 0:3852426a5068 373 **/
fablabtruck 0:3852426a5068 374 void LaosMotion::setOrigin(int x, int y, int z)
fablabtruck 0:3852426a5068 375 {
fablabtruck 0:3852426a5068 376 ofsx = x;
fablabtruck 0:3852426a5068 377 ofsy = y;
fablabtruck 0:3852426a5068 378 ofsz = z;
fablabtruck 0:3852426a5068 379 }
fablabtruck 0:3852426a5068 380
fablabtruck 0:3852426a5068 381
fablabtruck 0:3852426a5068 382
fablabtruck 0:3852426a5068 383
fablabtruck 0:3852426a5068 384 /**
fablabtruck 0:3852426a5068 385 *** Home the axis, stop when both home switches are pressed
fablabtruck 0:3852426a5068 386 **/
fablabtruck 0:3852426a5068 387 void LaosMotion::home(int x, int y, int z)
fablabtruck 0:3852426a5068 388 {
fablabtruck 0:3852426a5068 389 int i=0;
fablabtruck 0:3852426a5068 390 printf("Homing %d,%d, %d with speed %d\n", x, y, z, cfg->homespeed);
fablabtruck 0:3852426a5068 391 xdir = cfg->xhomedir;
fablabtruck 0:3852426a5068 392 ydir = cfg->yhomedir;
fablabtruck 0:3852426a5068 393 zdir = cfg->zhomedir;
fablabtruck 0:3852426a5068 394 led1 = 0;
fablabtruck 0:3852426a5068 395 isHome = false;
fablabtruck 0:3852426a5068 396 printf("Home Z...\n\r");
fablabtruck 0:3852426a5068 397 if (cfg->autozhome) {
fablabtruck 0:3852426a5068 398 while ((zmin ^ cfg->zpol) && (zmax ^ cfg->zpol)) {
fablabtruck 0:3852426a5068 399 zstep = 0;
fablabtruck 0:3852426a5068 400 wait(cfg->homespeed/1E6);
fablabtruck 0:3852426a5068 401 zstep = 1;
fablabtruck 0:3852426a5068 402 wait(cfg->homespeed/1E6);
fablabtruck 0:3852426a5068 403 }
fablabtruck 0:3852426a5068 404 }
fablabtruck 0:3852426a5068 405 printf("Home XY...\n\r");
fablabtruck 0:3852426a5068 406 while ( 1 )
fablabtruck 0:3852426a5068 407 {
fablabtruck 0:3852426a5068 408 xstep = ystep = 0;
fablabtruck 0:3852426a5068 409 wait(cfg->homespeed/1E6);
fablabtruck 0:3852426a5068 410 xstep = xhome ^ cfg->xpol;
fablabtruck 0:3852426a5068 411 ystep = yhome ^ cfg->ypol;
fablabtruck 0:3852426a5068 412 wait(cfg->homespeed/1E6);
fablabtruck 0:3852426a5068 413
fablabtruck 0:3852426a5068 414 led2 = !xhome;
fablabtruck 0:3852426a5068 415 led3 = !yhome;
fablabtruck 0:3852426a5068 416 led4 = ((i++) & 0x10000);
fablabtruck 0:3852426a5068 417 if ( !(xhome ^ cfg->xpol) && !(yhome ^ cfg->ypol) )
fablabtruck 0:3852426a5068 418 {
fablabtruck 0:3852426a5068 419 setPosition(x,y,z);
fablabtruck 0:3852426a5068 420 moveTo(x,y,z);
fablabtruck 0:3852426a5068 421 isHome = true;
fablabtruck 0:3852426a5068 422 printf("Home done.\n\r");
fablabtruck 0:3852426a5068 423 return;
fablabtruck 0:3852426a5068 424 }
fablabtruck 0:3852426a5068 425 }
fablabtruck 0:3852426a5068 426
fablabtruck 0:3852426a5068 427 }
fablabtruck 0:3852426a5068 428
fablabtruck 0:3852426a5068 429
fablabtruck 0:3852426a5068 430
fablabtruck 0:3852426a5068 431