Final code for our 4180 Drawing Robot!
Dependencies: 4DGL-uLCD-SE gCodeParser mbed
drawBot.h
- Committer:
- jford38
- Date:
- 2014-04-30
- Revision:
- 2:ba15545a4ccf
- Parent:
- 1:ad895d72e9ed
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