Final code for our 4180 Drawing Robot!
Dependencies: 4DGL-uLCD-SE gCodeParser mbed
Diff: drawBot.h
- Revision:
- 1:ad895d72e9ed
- Parent:
- 0:40576dfac535
--- a/drawBot.h Tue Apr 29 21:51:29 2014 +0000 +++ b/drawBot.h Wed Apr 30 01:47:43 2014 +0000 @@ -7,9 +7,12 @@ #define PI 3.1415926535 #define DEG_PER_STEP 1.8 -#define INCHES_PER_SEG 4 +#define INCHES_PER_SEG .25 #define START_Y 10 +#define CW_ARC (1) +#define CCW_ARC (-1) + Serial pc(USBTX, USBRX); class DrawBot { @@ -19,6 +22,7 @@ ~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(); @@ -39,7 +43,7 @@ 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) { @@ -53,7 +57,7 @@ this->width = width; px = width/2.0; py = START_Y; - step_delay = 80; + step_delay = 240; } DrawBot::~DrawBot() { @@ -168,7 +172,15 @@ 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. @@ -176,10 +188,14 @@ // 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) { +void DrawBot::arc(float cx,float cy,float x,float y,float dir) { + + cx += px; + cy += py; + // get radius - float dx = posx - cx; - float dy = posy - cy; + float dx = px - cx; + float dy = py - cy; float radius=sqrt(dx*dx+dy*dy); // find angle of arc (sweep) @@ -198,9 +214,9 @@ // simplifies to float len = abs(theta) * radius; - int i, segments = floor( len / CM_PER_SEGMENT ); + int i, segments = floor( len / INCHES_PER_SEG ); - float nx, ny, nz, angle3, scale; + float nx, ny, angle3, scale; for(i=0;i<segments;++i) { // interpolate around the arc @@ -209,12 +225,11 @@ 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(nx,ny); } - line(x,y,z); -}*/ + line(x,y); +} #endif \ No newline at end of file