Revision 0:aaad58ee9945, committed 2011-10-04

Comitter:

aaronbot3000

Date:

Tue Oct 04 02:36:37 2011 +0000

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,86 @@
+#ifndef COMMON_H
+#define COMMON_H
+
+#include "mbed.h"
+#include "math.h"
+ 
+#define ARM_UPPER_LEN 2.75
+#define ARM_LOWER_LEN 11.71875
+
+#define HAND_XOFF  1.09725
+#define HAND_ZOFF -0.20975
+
+#define TOOL_ZOFF -1.78000
+
+#define SERVO_XOFF -4.0096
+#define SERVO_ZOFF  0.7087
+
+#define MAX_Z 13
+#define MIN_Z 9
+
+#define MAX_X  3.1
+#define MIN_X -3.1
+
+#define MAX_Y  3.1
+#define MIN_Y -3.1
+
+#define START_Z (MIN_Z - 0.5)
+
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define RESTRICT(a, b1, b2) (MIN(b2, MAX(a, b1)))
+#define MAP(a, b1, b2, x1, x2) (((F32)((a) - (b1)) / (F32)((b2) - (b1))) * ((x2) - (x1)) + (x1)) 
+#define MAPEXP(a, b1, b2, x1, x2) (x1 * pow((float)1.5, -(((F32)((a) - (b1)) / (F32)((b2) - (b1))) * x2)))
+
+#define DEBUG
+
+typedef enum {
+    SUCCESS,
+    FAILURE,
+    END_PAT,
+    RUN_MORE
+} Status;
+
+typedef unsigned char  U08;
+typedef unsigned short U16;
+typedef unsigned int U32;
+typedef unsigned long long U64;
+
+typedef signed char  S08;
+typedef short S16;
+typedef int S32;
+typedef long long S64;
+
+typedef float F32;
+typedef double F64;
+
+struct Point {
+    F32 x;
+    F32 y;
+    F32 z;
+    
+    Point& operator=(const Point &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        return *this;
+    }
+    
+    Point(volatile Point& in) {
+        x = in.x;
+        y = in.y;
+        z = in.z;
+    }
+    
+    Point() {
+        x = y = z = 0;
+    }
+    
+    Point(F32 a, F32 b, F32 c) {
+        x = a;
+        y = b;
+        z = c;
+    }
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kinematics.cpp	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,70 @@
+#include "kinematics.h"
+
+Point rotate_xy(Point coord, F32 sin_theta, F32 cos_theta) {
+    Point ret;
+    ret.x = coord.x * cos_theta - coord.y * sin_theta;
+    ret.y = coord.x * sin_theta + coord.y * cos_theta;
+    ret.z = coord.z;
+    return ret;
+}
+
+inline F32 r2(F32 in) {
+    return in * in;
+}
+
+extern Serial pc;
+
+Status inv_kinematics(F32* result, Point target) {
+    F32 dist, inv_dist, alpha;
+    F32 angle;
+    F32 x1, z1, x2, z2, h;
+    F32 lower_radius;
+    Point target_rot, trans;
+
+    target_rot.x = target.x;
+    target_rot.y = target.y;
+    target_rot.z = target.z;
+
+    for (int i = 0; i < 3; i++) {
+        // rotate coordinates
+        if (i == 1)
+            target_rot = rotate_xy(target, SIN_120, COS_120);
+        if (i == 2)
+            target_rot = rotate_xy(target, SIN_240, COS_240);
+
+        // Add servo offset and hand offset
+        trans.x = target_rot.x + SERVO_XOFF + HAND_XOFF;
+        trans.y = target_rot.y;
+        // Add servo offset and tool offset
+        trans.z = target_rot.z + SERVO_ZOFF;
+
+        lower_radius = sqrt(r2(ARM_LOWER_LEN) - r2(trans.y));
+
+        dist     = sqrt(r2(trans.x) + r2(trans.z));
+        // Inverse square root!!!
+        inv_dist = 1 / dist;
+
+        // Bounds checking
+        if (dist > (ARM_UPPER_LEN + lower_radius) || 
+                dist < (lower_radius - ARM_UPPER_LEN)) {
+            pc.printf("OH FFFFFFUUUUUUUU Inv Failure %.5f, %.5f, %.5f\r\n", dist, ARM_UPPER_LEN, lower_radius);
+            //led2 = 1;
+            return FAILURE;
+        }
+
+        alpha = (r2(ARM_UPPER_LEN) - r2(lower_radius) + r2(dist)) * 0.5 * inv_dist;
+
+        x1 = (trans.x * alpha * inv_dist);
+        z1 = (trans.z * alpha * inv_dist);
+
+        h = sqrt(r2(ARM_UPPER_LEN) - r2(alpha));
+
+        x2 = -trans.z * (h * inv_dist);
+        z2 =  trans.x * (h * inv_dist);
+
+        angle = atan2(z1 - z2, x1 - x2);
+
+        result[i] = angle;
+    }
+    return SUCCESS;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kinematics.h	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,17 @@
+#ifndef KINEMATICS_H
+#define KINEMATICS_H
+
+#include "mbed.h"
+#include "common.h"
+
+#define COS_30   0.866025404
+#define TAN_60   0.577350269
+#define SIN_120  0.866025404
+#define COS_120 -0.5
+#define SIN_240 -0.866025404
+#define COS_240 -0.5
+
+Status inv_kinematics(F32* result, Point target);
+Status fwd_kinematics(Point* target, F32* angles);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+
+#include "common.h"
+#include "plan-position.h"
+#include "patterns.h"
+
+#define moves_z (MAX(draw_z - 0.5, MIN_Z - 0.5))
+
+#define START_TRANS 'B'
+#define END_TRANS 0xFFFF1111
+
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalIn goto_draw_height(p26);
+DigitalIn troll_up(p25);
+DigitalIn troll_down(p24);
+DigitalIn start_pat(p23);
+
+volatile U08 serial_buffer[16];
+volatile S32 sbuffer_index;
+
+static F32 draw_z = 9.5; // inches
+bool pen_needs_reset = false;
+
+Planner planner;
+
+void serial_callback() {
+    serial_buffer[sbuffer_index++] = pc.getc();
+}
+
+void setup() {
+    sbuffer_index = 0;
+    pc.baud(115200);
+    pc.attach(serial_callback);
+
+    setup_planner(&planner);
+
+    pc.printf("Setup\r\n");
+}
+
+Status wait_for_pattern() {
+    while (true) {
+        if (planner.finished) {
+            return SUCCESS;
+        }
+        if (planner.errored) {
+            pc.printf("FAILURE");
+            led2 = 1;
+            return FAILURE;
+        }
+    }
+}
+
+Status fill_buffer() {
+    int i;
+    Point in;
+    
+    pause_steppers(&planner);
+    pc.putc(START_TRANS);
+    for (i = 0; i < BUFFER_SIZE - 5; i++) {
+        while (sbuffer_index < 9);
+        if ((*(U32*)(&serial_buffer[0])) == END_TRANS)
+            return END_PAT;
+        sbuffer_index = 0;
+        pc.putc(START_TRANS);
+        
+        in.x = *(F32*)(&serial_buffer[0]);
+        in.y = *(F32*)(&serial_buffer[4]);
+        
+        if (serial_buffer[8] == 1)
+            in.z = moves_z;
+        else
+            in.z = draw_z;
+        
+        add_point_to_buffer(&planner, in);
+    }
+    resume_steppers(&planner);
+    
+    return SUCCESS;
+}
+
+void adj_z() {
+    Point next_pos = planner.current_pos;
+    if (troll_up) {
+        pc.printf("up\r\n");
+        draw_z -= 0.001; 
+        next_pos.z = draw_z;
+        add_point_to_buffer(&planner, next_pos);
+    }
+    else if (troll_down) {
+        pc.printf("down\r\n");
+        draw_z += 0.001; 
+        next_pos.z = draw_z;
+        add_point_to_buffer(&planner, next_pos);
+    }
+    else {
+        next_pos.z = draw_z;
+        add_point_to_buffer(&planner, next_pos);
+    }
+    wait_for_pattern();
+    pen_needs_reset = true;
+}
+
+void reset_pen() {
+    pc.printf("reset\r\n");
+    Point next_pos(0, 0, START_Z); 
+    add_point_to_buffer(&planner, next_pos);
+    wait_for_pattern();
+}
+
+int main() {
+    //Status status;
+    setup();
+
+    // adjust z
+    while (1) {
+        if (goto_draw_height) {
+            adj_z();
+        }
+        else if (start_pat) {
+            break;
+        }
+        else if (pen_needs_reset) {
+            reset_pen();
+            pen_needs_reset = false;
+        }
+    }
+    
+    wait_ms(500);
+    
+    while (true) {
+        while (!start_pat);
+        pause_steppers(&planner);
+        pc.printf("starting pattern\r\n");
+        draw_square_large(moves_z, draw_z, &planner);
+        resume_steppers(&planner);
+        wait_for_pattern();
+    }
+    
+    /*
+    while (1) {
+
+        while(1) {
+            run_pattern();
+            if (go_run_pat)
+                break;
+        }
+        
+        status = SUCCESS;
+        sbuffer_index = 0;
+        while (status == SUCCESS) {
+            status = fill_buffer();
+            run_pattern();
+        }
+        pc.putc('D');
+    }
+    */
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/patterns.cpp	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,206 @@
+#include "patterns.h"
+
+int draw_star(F32 moves_z, F32 draw_z, Planner* planner) {
+    Point a, b, c, d, e, g;
+    g.x =  0;
+    g.y =  2;
+    g.z = moves_z;
+    a.x =  0;
+    a.y =  2;
+    a.z = draw_z;
+    b.x =  2;
+    b.y = -2;
+    b.z = draw_z;
+    c.x = -2;
+    c.y =  1;
+    c.z = draw_z;
+    d.x =  2;
+    d.y =  1;
+    d.z = draw_z;
+    e.x = -2;
+    e.y = -2;
+    e.z = draw_z;
+    int index = 0;
+    add_point_to_buffer(planner, g);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, b);
+    add_point_to_buffer(planner, c);
+    add_point_to_buffer(planner, d);
+    add_point_to_buffer(planner, e);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, g);
+    return index;
+}
+
+int draw_square_large(F32 moves_z, F32 draw_z, Planner* planner) {
+    int index = 0;
+    Point a, b, c, d, g;
+    g.x =  0;
+    g.y =  4;
+    g.z = moves_z;
+    a.x =  0;
+    a.y =  4;
+    a.z = draw_z;
+    b.x =  4;
+    b.y =  0;
+    b.z = draw_z;
+    c.x =  0;
+    c.y = -4;
+    c.z = draw_z;
+    d.x = -4;
+    d.y =  0;
+    d.z = draw_z;
+
+    add_point_to_buffer(planner, g);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, b);
+    add_point_to_buffer(planner, c);
+    add_point_to_buffer(planner, d);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, g);
+    return index;
+}
+
+int draw_square_nn(F32 moves_z, F32 draw_z, Planner* planner) {
+    int index = 0;
+    Point a, b, c, d, g;
+    g.x = -3;
+    g.y = -3;
+    g.z = moves_z;
+    a.x = -3;
+    a.y = -3;
+    a.z = draw_z;
+    b.x = -1;
+    b.y = -3;
+    b.z = draw_z;
+    c.x = -1;
+    c.y = -1;
+    c.z = draw_z;
+    d.x = -3;
+    d.y = -1;
+    d.z = draw_z;
+
+    add_point_to_buffer(planner, g);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, b);
+    add_point_to_buffer(planner, c);
+    add_point_to_buffer(planner, d);
+    add_point_to_buffer(planner, a);
+    add_point_to_buffer(planner, g);
+    return index;
+}
+
+int draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner){
+    int index = 0;
+    Point a;
+    // H
+    a.x = 0; a.y = 0; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0; a.y = 2; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0; a.y = 1; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0; a.y = 1; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 1; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 1; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 2; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.5; a.y = 0; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    
+    // I
+    a.x = 0.75; a.y = 2; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 1.5; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 1.5; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 1; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 1; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 0.75; a.y = 0; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    
+    // T
+    a.x = 1.5; a.y = 2; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 1.5; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 1.75; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 1.75; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 1.75; a.y = 0; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    
+    // I
+    a.x = 2.25; a.y = 0; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.25; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.75; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.5; a.y = 0; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.5; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.25; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.75; a.y = 2; a.z = draw_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    a.x = 2.75; a.y = 2; a.z = moves_height;
+    a.x += off.x; a.y += off.y;
+    add_point_to_buffer(planner, a);
+    return index;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/patterns.h	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,12 @@
+#ifndef PATTERNS_H
+#define PATTERNS_H
+
+#include "common.h"
+#include "plan-position.h"
+
+int draw_star(F32 moves_height, F32 draw_height, Planner* planner);
+int draw_square_large(F32 moves_height, F32 draw_height, Planner* planner);
+int draw_square_nn(F32 moves_height, F32 draw_height, Planner* planner);
+int draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/plan-position.cpp	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,266 @@
+#include "plan-position.h"
+
+extern Serial pc;
+extern DigitalOut led1;
+extern DigitalOut led2;
+extern DigitalOut led4;
+
+static Planner* cur_plan;
+
+F32 dist_between(Point a, Point b) {
+    return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
+}
+
+void conform_goal(Point* in) {
+    in->x = RESTRICT(in->x, MIN_X, MAX_X);
+    in->y = RESTRICT(in->y, MIN_Y, MAX_Y);
+    in->z = RESTRICT(in->z, MIN_Z, MAX_Z);
+}
+
+F32 max(F32 a, F32 b) {
+    return (a > b ? a : b);
+}
+
+F32 max(F32 a, F32 b, F32 c) {
+    a = fabs(a);
+    b = fabs(b);
+    c = fabs(c);
+    if (a > b) {
+        return b > c ? a : a > c ? a : c;
+    }
+    else { // b > a
+        return a > c ? b : b > c ? b : c;
+    }
+}
+
+void setup_planner(Planner* planner) {
+    update_pos();
+    planner->angles_actual = get_angles();
+    
+    planner->buf_ind = 0;
+    planner->buf_next = 1;
+
+    planner->steps_to_next = 0;
+    
+    planner->buffer[0].x = 0;
+    planner->buffer[0].y = 0;
+    planner->buffer[0].z = START_Z;
+    
+    planner->current_pos.x = 0;
+    planner->current_pos.y = .1;
+    planner->current_pos.z = START_Z;
+
+    cur_plan = planner;
+
+    planner->finished = false;
+    planner->errored = false;
+
+    resume_steppers(planner);
+    planner->state = PLR_ACCL;
+}
+
+bool add_point_to_buffer(Planner* planner, Point in) {
+    if (INC_ONE(planner->buf_next) == planner->buf_ind)
+        return false;
+    planner->buffer[planner->buf_next].x = in.x;
+    planner->buffer[planner->buf_next].y = in.y;
+    planner->buffer[planner->buf_next].z = in.z;
+    planner->buf_next = INC_ONE(planner->buf_next);
+    planner->finished = false;
+    return true;
+}
+
+void pause_steppers(Planner* planner) {
+    planner->runner.detach();
+}
+
+void resume_steppers(Planner* planner) {
+    planner->runner.attach_us(take_step, TIMING_INTERVAL);
+}
+
+Status get_next_steps(Planner* planner, bool reset_dist_flag) {
+    Point goal = planner->buffer[planner->buf_ind];
+    Point cur_pos = planner->current_pos;
+
+    conform_goal(&goal);
+
+    F32 dx = goal.x - cur_pos.x;
+    F32 dy = goal.y - cur_pos.y;
+    F32 dz = goal.z - cur_pos.z;
+
+    F32 dist = dist_between(cur_pos, goal);
+    F32 step = 0;
+    Point test;
+
+    if (reset_dist_flag) {
+        planner->full_dist = dist;
+        planner->prev_dist = dist;
+        /*
+        pause_steppers(planner);
+        update_pos();
+        resume_steppers(planner);
+        */
+        for (int i = 0; i < 3; i++)
+            planner->angles_ideal[i] = planner->angles_actual[i];
+    }
+    F32 full_dist = planner->full_dist;
+    F32 prev_dist = planner->prev_dist;
+
+    F32 new_angles[3];
+
+    if (dist < MIN_DIST)
+        return END_PAT;
+
+    // Inverse square root!!!
+    F32 inv_vec_mag = 1 / dist;
+    dx *= inv_vec_mag;
+    dy *= inv_vec_mag;
+    dz *= inv_vec_mag;
+    
+    // Apply acceleration and deceleration and
+    // acquire next step size
+    if (planner->state == PLR_ACCL) {
+        if (full_dist - prev_dist > ACCL_ZONE)
+            planner->state = PLR_FULL;
+
+        else if (prev_dist < ACCL_ZONE && prev_dist * 2 < full_dist)
+            planner->state = PLR_DECL;
+
+        else
+            step = MAPEXP(full_dist - prev_dist, 0, ACCL_ZONE, MAX_STEP_SIZE, MIN_STEP_FRAC);
+    }
+    if (planner->state == PLR_FULL) {
+        if (prev_dist < ACCL_ZONE)
+            planner->state = PLR_DECL;
+
+        else
+            step = MAX_STEP_SIZE;
+    }
+    if (planner->state == PLR_DECL) {
+        Point test;
+        step = MAPEXP(ACCL_ZONE - (prev_dist), 0, ACCL_ZONE, MAX_STEP_SIZE, MIN_STEP_FRAC);
+        //step = MAP(prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX
+
+        test.x = cur_pos.x + dx * step;
+        test.y = cur_pos.y + dy * step;
+        test.z = cur_pos.z + dz * step;
+        
+        F32 tdist = dist_between(test, goal);
+        if (tdist >= prev_dist || tdist < MIN_DIST) {
+            planner->state = PLR_ACCL;
+            return END_PAT;
+        }
+    }
+    
+    // Generate a step
+    cur_pos.x += dx * step;
+    cur_pos.y += dy * step;
+    cur_pos.z += dz * step;
+    planner->current_pos = cur_pos;
+    
+    /*
+    pc.printf("goal: %f %f %f\r\n", goal.x, goal.y, goal.z);
+    pc.printf("curr: %f %f %f\r\n", cur_pos.x, cur_pos.y, cur_pos.z);
+    pc.printf("d,step: %f %f %f %f\r\n", dx * step, dy, dz, step);
+    pc.printf("dist, prev: %f %f\r\n", dist, prev_dist);
+    */
+
+    if (inv_kinematics(new_angles, cur_pos) != SUCCESS) {
+        led2 = 1;
+        return FAILURE;
+    }
+    
+    //pc.printf("newangles: %f %f %f\r\n", new_angles[0], new_angles[1], new_angles[2]);
+
+    // Get the difference in angles
+    for (int i = 0; i < 3; i++) {
+        //planner->angles_ideal[i] = planner->angles_actual[i];
+        new_angles[i] -= planner->angles_actual[i];
+    }
+
+    F32 max_diff = max(new_angles[0], new_angles[1], new_angles[2]);
+
+    // Number of steps to move max_diff amount
+    planner->steps_to_next = floor(max_diff / STEPPER_STEP_SIZE + 0.500001);
+    if (planner->steps_to_next <= 0) {
+        return SUCCESS;
+    }
+    
+    //pc.printf("max, steps, size, raw %f %d %f %f\r\n", max_diff, planner->steps_to_next, STEPPER_STEP_SIZE, max_diff / STEPPER_STEP_SIZE + 0.500001);
+    
+    for (int i = 0; i < 3; i++) {
+        planner->angles_step[i] = new_angles[i] / planner->steps_to_next;
+    }
+    
+    planner->prev_dist = dist;
+
+    return SUCCESS;
+}
+
+Status make_next_step(Planner* planner) {
+    S32 make_step = 0;
+    S32 direction = 0;
+    if (planner->steps_to_next == 0)
+        return SUCCESS;
+    //static int counter = 0;
+    //if (counter % 128 == 0) {
+    //    counter = 1;
+    //}
+    
+    // For all steppers, if the ideal angles are 1 step or greater from the actual angles,
+    // move the steppers to them.
+    for (int i = 0; i < 3; i++) {
+        planner->angles_ideal[i] += planner->angles_step[i];
+        
+        // Set the bits for the stepper mover
+        if (fabs(planner->angles_ideal[i] - planner->angles_actual[i]) >= STEPPER_STEP_SIZE) {
+            make_step |= 1 << i;
+            direction |= (planner->angles_ideal[i] > planner->angles_actual[i]) << i;
+        }
+    }
+    
+    if (move_steppers(make_step, direction) != SUCCESS) {
+        pc.printf("FFFUUUUU Stepper failure\r\n");
+        pc.printf("anglesa: %f %f %f\r\n", planner->angles_actual[0], planner->angles_actual[1], planner->angles_actual[2]);
+        pc.printf("anglesi: %f %f %f\r\n", planner->angles_ideal[0], planner->angles_ideal[1], planner->angles_ideal[2]);
+        pc.printf("position: %f %f %f\r\n", planner->current_pos.x, planner->current_pos.y, planner->current_pos.z);
+        return FAILURE;
+    }
+    planner->steps_to_next--;
+    return SUCCESS;
+}
+
+Timer timer;
+void take_step() {
+    bool reset_dist_flag;
+    static bool firstrun = true;
+    if (cur_plan->finished)
+        return;
+    //timer.reset();
+    //timer.start();
+    led1 = 0;
+    make_next_step(cur_plan);
+    if (cur_plan->steps_to_next == 0) {
+        if (cur_plan->buf_ind == cur_plan->buf_next) {
+            cur_plan->finished = true;
+            return;
+        }
+        reset_dist_flag = 0;
+        if (firstrun) {
+            firstrun = false;
+            reset_dist_flag = 1;
+            cur_plan->buf_ind = INC_ONE(cur_plan->buf_ind);
+        }
+        while (get_next_steps(cur_plan, reset_dist_flag) == END_PAT) {
+            led1 = 1;
+            reset_dist_flag = 1;
+            cur_plan->buf_ind = INC_ONE(cur_plan->buf_ind);
+            if (cur_plan->buf_ind == cur_plan->buf_next) {
+                cur_plan->finished = true;
+                return;
+            }
+        }
+    }
+    //timer.stop();
+    //pc.printf("Timer: %d\r\n", timer.read_us());
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/plan-position.h	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,56 @@
+#ifndef PLAN_POSITION_H
+#define PLAN_POSITION_H
+
+#include "common.h"
+#include "kinematics.h"
+#include "stepper.h"
+#include <cmath>
+
+#define BUFFER_SIZE 256
+#define INC_ONE(a) (((a) + 1) % BUFFER_SIZE)
+
+#define MAX_STEP_SIZE   0.030
+#define MIN_STEP_FRAC   3
+#define MIN_STEP_SIZE   0.0001
+
+#define MIN_DIST 0.02
+
+#define ACCL_ZONE 0.5 // inches
+
+#define TIMING_INTERVAL 1000
+
+enum Planner_State {
+    PLR_ACCL,
+    PLR_FULL,
+    PLR_DECL,
+    PLR_NEXT,
+    PLR_WAIT
+};
+
+struct Planner {
+    Ticker runner;
+    volatile Point buffer[BUFFER_SIZE];
+    volatile S32 buf_ind;
+    volatile S32 buf_next;
+
+    Planner_State state;
+    S32 steps_to_next;
+    F32 angles_step[3];
+    F32* angles_actual;
+    F32 angles_ideal[3];
+    Point current_pos;
+
+    F32 full_dist;
+    F32 prev_dist;
+
+    volatile bool finished;
+    volatile bool errored;
+};
+
+void setup_planner(Planner* planner);
+bool add_point_to_buffer(Planner* planner, Point in);
+void pause_steppers(Planner* planner);
+void resume_steppers(Planner* planner);
+void take_step();
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper.cpp	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,100 @@
+#include "stepper.h"
+
+static DigitalOut stepper[] = {DigitalOut(p10), DigitalOut(p11), DigitalOut(p12)};
+static DigitalOut dir[] = {DigitalOut(p13), DigitalOut(p14), DigitalOut(p15)};
+static AnalogIn posIn[] = {AnalogIn(p18), AnalogIn(p19), AnalogIn(p20)};
+
+extern DigitalOut led1;
+extern DigitalOut led2;
+extern DigitalOut led3;
+extern DigitalOut led4;
+
+static F32 angles[3];
+
+/*
+1:
+-.1184589 -> 36930
+1.4523373 -> 14880
+mult = 0.00007123792865
+2:
+-.1184589 -> 40450
+1.4523373 -> 15730
+mult = 0.00006354354072
+3:
+-.1184589 -> 41800
+1.4523373 -> 17600
+mult = 0.00006490893912
+*/
+
+static const F32 p_offset = -0.1184589;
+static const F32 p_ref[3] = {36930, 40450, 41800};
+static const F32 p_mult[3] = {0.00007123792865, 0.00006354354072, 0.00006490893912};
+static const S32 samples = 24;
+
+void update_pos() {
+    int i;
+    S32 ext[3][samples];
+    for (int x = 0; x < 3; x++) {
+        for (int j = 1; j < samples; j++) {
+            ext[x][j] = posIn[x].read_u16();
+            S32 index = ext[x][j];
+            for  (i = j; i > 0 && ext[x][i] > ext[x][i - 1]; i--) {
+                ext[x][i] = ext[x][i-1];
+            }
+            ext[x][i] = index;
+            wait_us(6);
+        }
+        for (int i = 8; i < 16; i++)
+            angles[x] += ext[x][i];
+        angles[x] /= 8;
+        angles[x] = (p_ref[x] - angles[x]) * p_mult[x] + p_offset;
+    }
+}
+
+F32* get_angles() {
+    return angles;
+}
+
+extern Serial pc;
+Status move_steppers(int steppers, int direction) {
+    Status retcode = SUCCESS;
+    for (int i = 0; i < 3; i++) {
+        // Step to larger angle (down)
+        if ((steppers >> i) & 0x1) {
+            if ((direction >> i) & 0x1) {
+                if (angles[i] > STEPPER_MAX_ANGLE) {
+                    retcode = FAILURE;
+                    steppers &= ~(0x1 << i);
+                    led2 = 1;
+                }
+                else {
+                    dir[i] = 0;
+                    angles[i] += STEPPER_STEP_SIZE;
+                }
+            }
+            // Step to smaller angle (up)
+            else {
+                if (angles[i] < STEPPER_MIN_ANGLE) {
+                    retcode = FAILURE;
+                    steppers &= ~(0x1 << i);
+                    led2 = 1;
+                }
+                else {
+                    dir[i] = 1;
+                    angles[i] -= STEPPER_STEP_SIZE;
+                }
+            }
+        }
+    }
+    wait_us(5);
+
+    for (int i = 0; i < 3; i++) {
+        stepper[i] = (steppers >> i) & 0x1;
+    }
+    wait_us(5);
+    
+    for (int i = 0; i < 3; i++) {
+        stepper[i] = 0;
+    }
+    return retcode;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper.h	Tue Oct 04 02:36:37 2011 +0000
@@ -0,0 +1,23 @@
+#ifndef STEPPER_H
+#define STEPPER_H
+
+#include "mbed.h"
+#include "common.h"
+
+#define STEPPER_MIN_ANGLE -1.01922088
+#define STEPPER_MAX_ANGLE  1.49077498
+#define STEPPER_STEP_SIZE  0.00305432619
+
+#define STEPPER_UP 1
+#define STEPPER_DN 0
+
+#ifdef DEBUG
+extern Serial pc;
+#endif
+
+void reset_steppers();
+void update_pos();
+F32* get_angles();
+Status move_steppers(int steppers, int directions);
+
+#endif