Michael Spencer / Mbed 2 deprecated LaOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

LaosMotion/LaosMotion.cpp@1:f5ac63519541, 2014-03-05 (annotated)

Committer:
Michael J. Spencer
Date:
Wed Mar 05 06:14:02 2014 -0800
Revision:
1:f5ac63519541
Initial commit.

Who changed what in which revision?

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