00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <inttypes.h>
00004 #include <avr/pgmspace.h>
00005
00006 #include "motori.h"
00007 #include "shvars.h"
00008
00009 static uint8_t moving = 0;
00010 static uint8_t zteep;
00011
00012 void move_reset() {
00013 moving = 0;
00014 }
00015
00016 uint8_t motors_ready() {
00017 return !moving;
00018 }
00019
00020 uint8_t move_is_steep() {
00021 return zteep;
00022 }
00023
00024 #define SWAP(a,b) {(a)^=(b);(b)^=(a);(a)^=(b);}
00025
00026 int16_t movestep(int16_t x1, int16_t y1) {
00027 static int16_t twoDy, twoDyTwoDx, E, x, y, xend, stepcount;
00028 static int8_t xstep, ystep;
00029 static uint8_t skippy;
00030
00031 static int stepX, stepY;
00032
00033
00034 if (!moving && x1 != -1 && y1 != -1) {
00035 int x0 = stepper_loc.x;
00036 int y0 = stepper_loc.y;
00037 int Dx = x1 - x0;
00038 int Dy = y1 - y0;
00039
00040 zteep = abs(Dy) >= abs(Dx);
00041
00042 if (zteep) {
00043 SWAP(x0, y0);
00044 SWAP(x1, y1);
00045
00046 Dx = x1 - x0;
00047 Dy = y1 - y0;
00048 }
00049
00050 xstep = 1;
00051 if (Dx < 0) {
00052 xstep = -1;
00053 Dx = -Dx;
00054 }
00055 ystep = 1;
00056 if (Dy < 0) {
00057 ystep = -1;
00058 Dy = -Dy;
00059 }
00060
00061 twoDy = 2*Dy;
00062 twoDyTwoDx = twoDy - 2*Dx;
00063 E = twoDy - Dx;
00064 y = y0;
00065 x = x0;
00066 xend = x1;
00067
00068 moving = Dx != 0;
00069 skippy = 0;
00070
00071
00072
00073
00074 stepcount = 0;
00075 return Dx;
00076 }
00077
00078
00079
00080 if (moving) {
00081 #ifdef YSKIPPY
00082 skippy++;
00083
00084
00085
00086 if (zteep && ((skippy & 0x11) == 0x10)) {
00087 return -1;
00088 }
00089 #endif
00090 int makeYstep = 0;
00091
00092 if (E > 0) {
00093 E += twoDyTwoDx;
00094 y += ystep;
00095 makeYstep = 1;
00096 } else {
00097 E += twoDy;
00098 }
00099 x += xstep;
00100 stepcount++;
00101
00102 if (zteep) {
00103 stepper_loc.x = y;
00104 stepper_loc.y = x;
00105 stepX = makeYstep ? ystep : 0;
00106 stepY = xstep;
00107 } else {
00108 stepper_loc.x = x;
00109 stepper_loc.y = y;
00110 stepX = xstep;
00111 stepY = makeYstep ? ystep : 0;
00112 }
00113
00114 if (x == xend) {
00115 moving = 0;
00116 }
00117 step(stepX, stepY);
00118 }
00119
00120 return stepcount;
00121 }