drawBot.h

00001 #ifndef DRAWBOT_H
00002 #define DRAWBOT_H
00003 
00004 #include "mbed.h"
00005 #include "motor.h"
00006 #include "pen.h"
00007 
00008 #define PI 3.1415926535
00009 #define DEG_PER_STEP 1.8
00010 #define INCHES_PER_SEG .25
00011 #define START_Y 10
00012 
00013 #define CW_ARC (1)
00014 #define CCW_ARC (-1)
00015 
00016 Serial pc(USBTX, USBRX);
00017 
00018 class DrawBot {
00019     
00020     public: 
00021         DrawBot(Motor*, Motor*, PinName, float, float);
00022         ~DrawBot();
00023         void drawCrap();
00024         void line_safe(float x, float y);
00025         void arc(float cx,float cy,float x,float y,float dir);
00026         inline void pen_up();
00027         inline void pen_down();
00028         inline void disable();
00029         
00030     private:
00031         float px, py;       // Position
00032         float fr;           // Feed rate
00033         float step_delay;   // How long to delay between steps.  
00034         float rad;          // Pulley Radius (inches)  
00035         float STEP_PER_INCH;
00036         float width;        // Distance from Pulley to Pulley (inches)
00037         
00038         Motor* mL;
00039         Motor* mR; 
00040         
00041         Pen* pen;  
00042         
00043         inline void pause(long ms); 
00044         inline void IK(float x, float y, long &l1, long &l2);   
00045         void line(float newx,float newy);
00046         float atan3(float dy, float dx);
00047 };
00048 
00049 DrawBot::DrawBot(Motor* mL, Motor* mR, PinName s_pin, float rad, float width) {
00050     
00051     this->mL = mL;
00052     this->mR = mR;
00053     pen = new Pen(s_pin, 90);
00054     
00055     STEP_PER_INCH = mL->get_msLevel()*360.0/(DEG_PER_STEP * 2*PI * rad);
00056     this->rad = rad;
00057     this->width = width;
00058     px = width/2.0;
00059     py = START_Y;
00060     step_delay = 240;
00061 }
00062 
00063 DrawBot::~DrawBot() {
00064     delete mL;
00065     delete mR;
00066 }
00067 
00068 inline void DrawBot::pause(long us) {
00069     wait_ms(us/1000);
00070     wait_us(us%1000);    
00071 }
00072 
00073 inline void DrawBot::pen_up() {
00074     pen->up();
00075 }
00076 
00077 inline void DrawBot::pen_down() {
00078     pen->down();
00079 }
00080 
00081 inline void DrawBot::disable() {
00082     pen->up();
00083     mL->disable();
00084     mR->disable();   
00085 }
00086 
00087 inline void DrawBot::IK(float x, float y, long &l1, long &l2) {
00088     long w = width * STEP_PER_INCH;
00089     l1 = sqrt(x*x + y*y - rad*rad);
00090     l2 = sqrt(y*y + (w-x)*(w-x) - rad*rad);
00091 }
00092 
00093 void DrawBot::line(float newx, float newy) {
00094 
00095     pc.printf("LINE: (%f, %f) -> (%f, %f)\n",px,py,newx,newy);
00096 
00097     long L1, L2, oldL1, oldL2;
00098     IK(px*STEP_PER_INCH, py*STEP_PER_INCH, oldL1, oldL2);
00099     IK(newx*STEP_PER_INCH, newy*STEP_PER_INCH, L1, L2);
00100 
00101     long d1=L1-oldL1;
00102     long d2=L2-oldL2;
00103     int dir1=d1>0?1:0;
00104     int dir2=d2>0?1:0;  
00105     d1=abs(d1);
00106     d2=abs(d2);
00107     
00108     long i;
00109     long over = 0;
00110     
00111     if(d1>d2) {
00112         for(i=0;i<d1;++i) {
00113           mL->one_step(dir1);
00114           over+=d2;
00115           if(over>=d1) {
00116             over-=d1;
00117             mR->one_step(dir2);
00118           }
00119           pause(step_delay);
00120         }
00121     } else {
00122         for(i=0;i<d2;++i) {
00123           mR->one_step(dir2);
00124           over+=d1;
00125           if(over>=d2) {
00126             over-=d2;
00127             mL->one_step(dir1);
00128           }
00129           pause(step_delay);
00130         }
00131     }
00132     
00133     px=newx;
00134     py=newy;
00135 }
00136 
00137 void DrawBot::line_safe(float x,float y) {
00138   
00139   // split up long lines to make them straighter?
00140   float dx=x-px;
00141   float dy=y-py;
00142 
00143   float len=sqrt(dx*dx+dy*dy);
00144   
00145   if(len<=INCHES_PER_SEG) {
00146     line(x,y);
00147     return;
00148   }
00149   
00150   // too long!
00151   long pieces=floor(len/INCHES_PER_SEG);
00152   float x0=px;
00153   float y0=py;
00154 
00155   float a;
00156   for(long j=0;j<=pieces;++j) {
00157     a=(float)j/(float)pieces;
00158     
00159     line((x-x0)*a+x0,
00160          (y-y0)*a+y0);
00161   }
00162   line(x,y);
00163 }
00164 
00165 void DrawBot::drawCrap() {
00166     pc.printf("\nSTARTING\n");
00167     line_safe(width/2.0+4, START_Y);
00168     line_safe(width/2.0+4, START_Y+4);
00169     line_safe(width/2.0, START_Y+4);
00170     line_safe(width/2.0, START_Y);
00171 
00172     mL->disable();
00173     mR->disable();
00174 }
00175 
00176 //------------------------------------------------------------------------------
00177 // returns angle of dy/dx as a value from 0...2PI
00178 float DrawBot::atan3(float dy,float dx) {
00179   float a=atan2(dy,dx);
00180   if(a<0) a=(PI*2.0)+a;
00181   return a;
00182 }
00183 
00184 //------------------------------------------------------------------------------
00185 // This method assumes the limits have already been checked.
00186 // This method assumes the start and end radius match.
00187 // This method assumes arcs are not >180 degrees (PI radians)
00188 // cx/cy - center of circle
00189 // x/y - end position
00190 // dir - ARC_CW or ARC_CCW to control direction of arc
00191 void DrawBot::arc(float cx,float cy,float x,float y,float dir) {
00192   
00193   cx += px;
00194   cy += py;
00195   
00196   // get radius
00197   float dx = px - cx;
00198   float dy = py - cy;
00199   float radius=sqrt(dx*dx+dy*dy);
00200 
00201   // find angle of arc (sweep)
00202   float angle1=atan3(dy,dx);
00203   float angle2=atan3(y-cy,x-cx);
00204   float theta=angle2-angle1;
00205   
00206   if(dir>0 && theta<0) angle2+=2*PI;
00207   else if(dir<0 && theta>0) angle1+=2*PI;
00208   
00209   theta=angle2-angle1;
00210   
00211   // get length of arc
00212   // float circ=PI*2.0*radius;
00213   // float len=theta*circ/(PI*2.0);
00214   // simplifies to
00215   float len = abs(theta) * radius;
00216 
00217   int i, segments = floor( len / INCHES_PER_SEG );
00218  
00219   float nx, ny, angle3, scale;
00220 
00221   for(i=0;i<segments;++i) {
00222     // interpolate around the arc
00223     scale = ((float)i)/((float)segments);
00224     
00225     angle3 = ( theta * scale ) + angle1;
00226     nx = cx + cos(angle3) * radius;
00227     ny = cy + sin(angle3) * radius;
00228     // send it to the planner
00229     line(nx,ny);
00230   }
00231   
00232   line(x,y);
00233 }
00234 
00235 #endif