Diff: drawBot.h

Revision:

0:40576dfac535

Child:

1:ad895d72e9ed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drawBot.h	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,220 @@
+#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 4
+#define START_Y 10
+
+Serial pc(USBTX, USBRX);
+
+class DrawBot {
+    
+    public: 
+        DrawBot(Motor*, Motor*, PinName, float, float);
+        ~DrawBot();
+        void drawCrap();
+        void line_safe(float x, float y);
+        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);
+  
+};
+
+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 = 80;
+}
+
+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();
+}
+/*
+//------------------------------------------------------------------------------
+// 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
+static void arc(float cx,float cy,float x,float y,float z,float dir) {
+  // get radius
+  float dx = posx - cx;
+  float dy = posy - 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 / CM_PER_SEGMENT );
+ 
+  float nx, ny, nz, 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;
+    nz = ( z - posz ) * scale + posz;
+    // send it to the planner
+    line(nx,ny,nz);
+  }
+  
+  line(x,y,z);
+}*/
+
+#endif
\ No newline at end of file