peter brier
/
laos
LaOS Laser test program
main.cpp@0:5c352dab4496, 2011-01-27 (annotated)
- Committer:
- pbrier
- Date:
- Thu Jan 27 19:51:07 2011 +0000
- Revision:
- 0:5c352dab4496
- Child:
- 1:93d357b92fd0
1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pbrier | 0:5c352dab4496 | 1 | /* |
pbrier | 0:5c352dab4496 | 2 | * main.cpp |
pbrier | 0:5c352dab4496 | 3 | * |
pbrier | 0:5c352dab4496 | 4 | * Copyright (c) 2011 Peter Brier |
pbrier | 0:5c352dab4496 | 5 | * |
pbrier | 0:5c352dab4496 | 6 | * This file is part of the LaOS project (see: http://wiki.protospace.nl/index.php/LaOS) |
pbrier | 0:5c352dab4496 | 7 | * |
pbrier | 0:5c352dab4496 | 8 | * LaOS is free software: you can redistribute it and/or modify |
pbrier | 0:5c352dab4496 | 9 | * it under the terms of the GNU General Public License as published by |
pbrier | 0:5c352dab4496 | 10 | * the Free Software Foundation, either version 3 of the License, or |
pbrier | 0:5c352dab4496 | 11 | * (at your option) any later version. |
pbrier | 0:5c352dab4496 | 12 | * |
pbrier | 0:5c352dab4496 | 13 | * LaOS is distributed in the hope that it will be useful, |
pbrier | 0:5c352dab4496 | 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
pbrier | 0:5c352dab4496 | 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
pbrier | 0:5c352dab4496 | 16 | * GNU General Public License for more details. |
pbrier | 0:5c352dab4496 | 17 | * |
pbrier | 0:5c352dab4496 | 18 | * You should have received a copy of the GNU General Public License |
pbrier | 0:5c352dab4496 | 19 | * along with LaOS. If not, see <http://www.gnu.org/licenses/>. |
pbrier | 0:5c352dab4496 | 20 | * |
pbrier | 0:5c352dab4496 | 21 | * This program implements Bresenham line algorithm program (2D) |
pbrier | 0:5c352dab4496 | 22 | * see: http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm |
pbrier | 0:5c352dab4496 | 23 | * In addition: we controll a Z-axis and a laser. |
pbrier | 0:5c352dab4496 | 24 | * Note: Fixed speed lasering (no accelleration). Actually: vector speed depends on angle. Perpendicular to X or Y axis is lowest speed |
pbrier | 0:5c352dab4496 | 25 | * At 45 degrees is is sqrt(2) faster (approx 40%). Simple program, no line buffering. Considerable jitter on pulses. |
pbrier | 0:5c352dab4496 | 26 | * |
pbrier | 0:5c352dab4496 | 27 | */ |
pbrier | 0:5c352dab4496 | 28 | #include "mbed.h" |
pbrier | 0:5c352dab4496 | 29 | |
pbrier | 0:5c352dab4496 | 30 | // serial comm |
pbrier | 0:5c352dab4496 | 31 | Serial pc(USBTX, USBRX); |
pbrier | 0:5c352dab4496 | 32 | |
pbrier | 0:5c352dab4496 | 33 | // status leds |
pbrier | 0:5c352dab4496 | 34 | DigitalOut led1(LED1); |
pbrier | 0:5c352dab4496 | 35 | DigitalOut led2(LED2); |
pbrier | 0:5c352dab4496 | 36 | DigitalOut led3(LED3); |
pbrier | 0:5c352dab4496 | 37 | DigitalOut led4(LED4); |
pbrier | 0:5c352dab4496 | 38 | |
pbrier | 0:5c352dab4496 | 39 | // motors |
pbrier | 0:5c352dab4496 | 40 | DigitalOut xdir(p26); |
pbrier | 0:5c352dab4496 | 41 | DigitalOut xstep(p25); |
pbrier | 0:5c352dab4496 | 42 | DigitalOut ydir(p24); |
pbrier | 0:5c352dab4496 | 43 | DigitalOut ystep(p23); |
pbrier | 0:5c352dab4496 | 44 | DigitalOut zdir(p22); |
pbrier | 0:5c352dab4496 | 45 | DigitalOut zstep(p21); |
pbrier | 0:5c352dab4496 | 46 | |
pbrier | 0:5c352dab4496 | 47 | // laser |
pbrier | 0:5c352dab4496 | 48 | DigitalOut laser(p27); |
pbrier | 0:5c352dab4496 | 49 | |
pbrier | 0:5c352dab4496 | 50 | // globals |
pbrier | 0:5c352dab4496 | 51 | Ticker ticker; // Timer |
pbrier | 0:5c352dab4496 | 52 | |
pbrier | 0:5c352dab4496 | 53 | // Besenham variables |
pbrier | 0:5c352dab4496 | 54 | int state, laseron; |
pbrier | 0:5c352dab4496 | 55 | int x0,y0,x1,y1,z0,z1; // positions: 0=current, 1=new |
pbrier | 0:5c352dab4496 | 56 | int dx, sx; // step |
pbrier | 0:5c352dab4496 | 57 | int dy, sy; // delta |
pbrier | 0:5c352dab4496 | 58 | int err, e2; // error |
pbrier | 0:5c352dab4496 | 59 | |
pbrier | 0:5c352dab4496 | 60 | |
pbrier | 0:5c352dab4496 | 61 | /** |
pbrier | 0:5c352dab4496 | 62 | *** ticker: implement timer ticker, step if required |
pbrier | 0:5c352dab4496 | 63 | **/ |
pbrier | 0:5c352dab4496 | 64 | void tick() |
pbrier | 0:5c352dab4496 | 65 | { |
pbrier | 0:5c352dab4496 | 66 | // Control X,Y: line tracker |
pbrier | 0:5c352dab4496 | 67 | switch ( state ) |
pbrier | 0:5c352dab4496 | 68 | { |
pbrier | 0:5c352dab4496 | 69 | case 0: // idle (make sure laser is off) |
pbrier | 0:5c352dab4496 | 70 | laser = laseron = 0; |
pbrier | 0:5c352dab4496 | 71 | break; |
pbrier | 0:5c352dab4496 | 72 | case 1: // init |
pbrier | 0:5c352dab4496 | 73 | dx = abs(x1-x0); |
pbrier | 0:5c352dab4496 | 74 | sx = x0<x1 ? 1 : -1; |
pbrier | 0:5c352dab4496 | 75 | dy = abs(y1-y0); |
pbrier | 0:5c352dab4496 | 76 | sy = y0<y1 ? 1 : -1; |
pbrier | 0:5c352dab4496 | 77 | err = (dx>dy ? dx : -dy)/2; |
pbrier | 0:5c352dab4496 | 78 | xdir = sx > 0; |
pbrier | 0:5c352dab4496 | 79 | ydir = sy > 0; |
pbrier | 0:5c352dab4496 | 80 | state++; |
pbrier | 0:5c352dab4496 | 81 | laser = laseron; |
pbrier | 0:5c352dab4496 | 82 | break; |
pbrier | 0:5c352dab4496 | 83 | case 2: // moving |
pbrier | 0:5c352dab4496 | 84 | if (x0==x1 && y0==y1) |
pbrier | 0:5c352dab4496 | 85 | state = 0; |
pbrier | 0:5c352dab4496 | 86 | else |
pbrier | 0:5c352dab4496 | 87 | { |
pbrier | 0:5c352dab4496 | 88 | e2 = err; |
pbrier | 0:5c352dab4496 | 89 | if (e2 >-dx) { err -= dy; x0 += sx; xstep = 1;} |
pbrier | 0:5c352dab4496 | 90 | if (e2 < dy) { err += dx; y0 += sy; ystep = 1;} |
pbrier | 0:5c352dab4496 | 91 | } |
pbrier | 0:5c352dab4496 | 92 | break; |
pbrier | 0:5c352dab4496 | 93 | } |
pbrier | 0:5c352dab4496 | 94 | |
pbrier | 0:5c352dab4496 | 95 | // Controll Z: tracking controller (independent of XY motion, not queued) |
pbrier | 0:5c352dab4496 | 96 | if ( z1 != z0 ) |
pbrier | 0:5c352dab4496 | 97 | { |
pbrier | 0:5c352dab4496 | 98 | zdir = z1 > z0; |
pbrier | 0:5c352dab4496 | 99 | wait_us(5); |
pbrier | 0:5c352dab4496 | 100 | zstep= 1; |
pbrier | 0:5c352dab4496 | 101 | if ( z1 > z0 ) z0++; |
pbrier | 0:5c352dab4496 | 102 | else z0--; |
pbrier | 0:5c352dab4496 | 103 | } |
pbrier | 0:5c352dab4496 | 104 | |
pbrier | 0:5c352dab4496 | 105 | if ( xstep == 1 || ystep == 1 || zstep == 1 ) |
pbrier | 0:5c352dab4496 | 106 | { |
pbrier | 0:5c352dab4496 | 107 | wait_us(5); |
pbrier | 0:5c352dab4496 | 108 | xstep = ystep = zstep = 0; |
pbrier | 0:5c352dab4496 | 109 | } |
pbrier | 0:5c352dab4496 | 110 | |
pbrier | 0:5c352dab4496 | 111 | // diagnostic info |
pbrier | 0:5c352dab4496 | 112 | led1 = xdir; |
pbrier | 0:5c352dab4496 | 113 | led2 = ydir; |
pbrier | 0:5c352dab4496 | 114 | led3 = zdir; |
pbrier | 0:5c352dab4496 | 115 | led4 = laser; |
pbrier | 0:5c352dab4496 | 116 | } // ticker |
pbrier | 0:5c352dab4496 | 117 | |
pbrier | 0:5c352dab4496 | 118 | |
pbrier | 0:5c352dab4496 | 119 | |
pbrier | 0:5c352dab4496 | 120 | /** |
pbrier | 0:5c352dab4496 | 121 | *** main() |
pbrier | 0:5c352dab4496 | 122 | *** Read commands, report state |
pbrier | 0:5c352dab4496 | 123 | **/ |
pbrier | 0:5c352dab4496 | 124 | int main() |
pbrier | 0:5c352dab4496 | 125 | { |
pbrier | 0:5c352dab4496 | 126 | int v=100,c=0,z=0; |
pbrier | 0:5c352dab4496 | 127 | |
pbrier | 0:5c352dab4496 | 128 | pc.baud(115200); |
pbrier | 0:5c352dab4496 | 129 | ticker.attach_us(&tick, v); // the address of the function to be attached (flip) and the interval (microseconds) |
pbrier | 0:5c352dab4496 | 130 | |
pbrier | 0:5c352dab4496 | 131 | // init |
pbrier | 0:5c352dab4496 | 132 | state = laser = xstep = xdir = ystep = ydir = zstep = zdir = 0; |
pbrier | 0:5c352dab4496 | 133 | |
pbrier | 0:5c352dab4496 | 134 | while( 1 ) |
pbrier | 0:5c352dab4496 | 135 | { |
pbrier | 0:5c352dab4496 | 136 | scanf("%d", &c); |
pbrier | 0:5c352dab4496 | 137 | switch( c ) |
pbrier | 0:5c352dab4496 | 138 | { |
pbrier | 0:5c352dab4496 | 139 | case 0: // move x,y (laser off) |
pbrier | 0:5c352dab4496 | 140 | scanf("%d %d", &x1, &y1); |
pbrier | 0:5c352dab4496 | 141 | laseron = 0; |
pbrier | 0:5c352dab4496 | 142 | state = 1; |
pbrier | 0:5c352dab4496 | 143 | break; |
pbrier | 0:5c352dab4496 | 144 | case 1: // line x,y (laser on) |
pbrier | 0:5c352dab4496 | 145 | scanf("%d %d", &x1, &y1); |
pbrier | 0:5c352dab4496 | 146 | laseron = 1; |
pbrier | 0:5c352dab4496 | 147 | state = 1; |
pbrier | 0:5c352dab4496 | 148 | break; |
pbrier | 0:5c352dab4496 | 149 | case 2: // move z |
pbrier | 0:5c352dab4496 | 150 | scanf("%d", &z1); |
pbrier | 0:5c352dab4496 | 151 | break; |
pbrier | 0:5c352dab4496 | 152 | case 3: // set t (1/v) |
pbrier | 0:5c352dab4496 | 153 | scanf("%d", &v); |
pbrier | 0:5c352dab4496 | 154 | ticker.attach_us(&tick, v); |
pbrier | 0:5c352dab4496 | 155 | break; |
pbrier | 0:5c352dab4496 | 156 | case 4: // set x,y,z absolute |
pbrier | 0:5c352dab4496 | 157 | scanf("%d %d %d", &x0, &y0, &z); |
pbrier | 0:5c352dab4496 | 158 | z1 = z0 = z; |
pbrier | 0:5c352dab4496 | 159 | case 5: // nop |
pbrier | 0:5c352dab4496 | 160 | break; |
pbrier | 0:5c352dab4496 | 161 | default: // I do not understand: stop motion |
pbrier | 0:5c352dab4496 | 162 | state = 0; |
pbrier | 0:5c352dab4496 | 163 | break; |
pbrier | 0:5c352dab4496 | 164 | } |
pbrier | 0:5c352dab4496 | 165 | printf("%d %d %d %d %d %d\r\n", state, x0, y0, z0, v, laseron); |
pbrier | 0:5c352dab4496 | 166 | } |
pbrier | 0:5c352dab4496 | 167 | } |