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?
User | Revision | Line number | New 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 |