File content as of revision 2:ba15545a4ccf:

#ifndef DRAWBOT_H
#define DRAWBOT_H

#include "mbed.h"
#include "motor.h"
#include "pen.h"

#define PI 3.1415926535
#define DEG_PER_STEP 1.8
#define INCHES_PER_SEG .25
#define START_Y 10

#define CW_ARC (1)
#define CCW_ARC (-1)

Serial pc(USBTX, USBRX);

class DrawBot {
    
    public: 
        DrawBot(Motor*, Motor*, PinName, float, float);
        ~DrawBot();
        void drawCrap();
        void line_safe(float x, float y);
        void arc(float cx,float cy,float x,float y,float dir);
        inline void pen_up();
        inline void pen_down();
        inline void disable();
        
    private:
        float px, py;       // Position
        float fr;           // Feed rate
        float step_delay;   // How long to delay between steps.  
        float rad;          // Pulley Radius (inches)  
        float STEP_PER_INCH;
        float width;        // Distance from Pulley to Pulley (inches)
        
        Motor* mL;
        Motor* mR; 
        
        Pen* pen;  
        
        inline void pause(long ms); 
        inline void IK(float x, float y, long &l1, long &l2);   
        void line(float newx,float newy);
        float atan3(float dy, float dx);
};

DrawBot::DrawBot(Motor* mL, Motor* mR, PinName s_pin, float rad, float width) {
    
    this->mL = mL;
    this->mR = mR;
    pen = new Pen(s_pin, 90);
    
    STEP_PER_INCH = mL->get_msLevel()*360.0/(DEG_PER_STEP * 2*PI * rad);
    this->rad = rad;
    this->width = width;
    px = width/2.0;
    py = START_Y;
    step_delay = 240;
}

DrawBot::~DrawBot() {
    delete mL;
    delete mR;
}

inline void DrawBot::pause(long us) {
    wait_ms(us/1000);
    wait_us(us%1000);    
}

inline void DrawBot::pen_up() {
    pen->up();
}

inline void DrawBot::pen_down() {
    pen->down();
}

inline void DrawBot::disable() {
    pen->up();
    mL->disable();
    mR->disable();   
}

inline void DrawBot::IK(float x, float y, long &l1, long &l2) {
    long w = width * STEP_PER_INCH;
    l1 = sqrt(x*x + y*y - rad*rad);
    l2 = sqrt(y*y + (w-x)*(w-x) - rad*rad);
}

void DrawBot::line(float newx, float newy) {

    pc.printf("LINE: (%f, %f) -> (%f, %f)\n",px,py,newx,newy);

    long L1, L2, oldL1, oldL2;
    IK(px*STEP_PER_INCH, py*STEP_PER_INCH, oldL1, oldL2);
    IK(newx*STEP_PER_INCH, newy*STEP_PER_INCH, L1, L2);

    long d1=L1-oldL1;
    long d2=L2-oldL2;
    int dir1=d1>0?1:0;
    int dir2=d2>0?1:0;  
    d1=abs(d1);
    d2=abs(d2);
    
    long i;
    long over = 0;
    
    if(d1>d2) {
        for(i=0;i<d1;++i) {
          mL->one_step(dir1);
          over+=d2;
          if(over>=d1) {
            over-=d1;
            mR->one_step(dir2);
          }
          pause(step_delay);
        }
    } else {
        for(i=0;i<d2;++i) {
          mR->one_step(dir2);
          over+=d1;
          if(over>=d2) {
            over-=d2;
            mL->one_step(dir1);
          }
          pause(step_delay);
        }
    }
    
    px=newx;
    py=newy;
}

void DrawBot::line_safe(float x,float y) {
  
  // split up long lines to make them straighter?
  float dx=x-px;
  float dy=y-py;

  float len=sqrt(dx*dx+dy*dy);
  
  if(len<=INCHES_PER_SEG) {
    line(x,y);
    return;
  }
  
  // too long!
  long pieces=floor(len/INCHES_PER_SEG);
  float x0=px;
  float y0=py;

  float a;
  for(long j=0;j<=pieces;++j) {
    a=(float)j/(float)pieces;
    
    line((x-x0)*a+x0,
         (y-y0)*a+y0);
  }
  line(x,y);
}

void DrawBot::drawCrap() {
    pc.printf("\nSTARTING\n");
    line_safe(width/2.0+4, START_Y);
    line_safe(width/2.0+4, START_Y+4);
    line_safe(width/2.0, START_Y+4);
    line_safe(width/2.0, START_Y);

    mL->disable();
    mR->disable();
}

//------------------------------------------------------------------------------
// returns angle of dy/dx as a value from 0...2PI
float DrawBot::atan3(float dy,float dx) {
  float a=atan2(dy,dx);
  if(a<0) a=(PI*2.0)+a;
  return a;
}

//------------------------------------------------------------------------------
// This method assumes the limits have already been checked.
// This method assumes the start and end radius match.
// This method assumes arcs are not >180 degrees (PI radians)
// cx/cy - center of circle
// x/y - end position
// dir - ARC_CW or ARC_CCW to control direction of arc
void DrawBot::arc(float cx,float cy,float x,float y,float dir) {
  
  cx += px;
  cy += py;
  
  // get radius
  float dx = px - cx;
  float dy = py - cy;
  float radius=sqrt(dx*dx+dy*dy);

  // find angle of arc (sweep)
  float angle1=atan3(dy,dx);
  float angle2=atan3(y-cy,x-cx);
  float theta=angle2-angle1;
  
  if(dir>0 && theta<0) angle2+=2*PI;
  else if(dir<0 && theta>0) angle1+=2*PI;
  
  theta=angle2-angle1;
  
  // get length of arc
  // float circ=PI*2.0*radius;
  // float len=theta*circ/(PI*2.0);
  // simplifies to
  float len = abs(theta) * radius;

  int i, segments = floor( len / INCHES_PER_SEG );
 
  float nx, ny, angle3, scale;

  for(i=0;i<segments;++i) {
    // interpolate around the arc
    scale = ((float)i)/((float)segments);
    
    angle3 = ( theta * scale ) + angle1;
    nx = cx + cos(angle3) * radius;
    ny = cy + sin(angle3) * radius;
    // send it to the planner
    line(nx,ny);
  }
  
  line(x,y);
}

#endif