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