Jordan Ford / Mbed 2 deprecated DrawBot

Final code for our 4180 Drawing Robot!

Dependencies:   4DGL-uLCD-SE gCodeParser mbed

drawBot.h@2:ba15545a4ccf, 2014-04-30 (annotated)

Committer:
jford38
Date:
Wed Apr 30 16:40:10 2014 +0000
Revision:
2:ba15545a4ccf
Parent:
1:ad895d72e9ed 
Added Visual Studio Source files and README.; Alejandro.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jford38 0:40576dfac535 1 #ifndef DRAWBOT_H
jford38 0:40576dfac535 2 #define DRAWBOT_H
jford38 0:40576dfac535 3
jford38 0:40576dfac535 4 #include "mbed.h"
jford38 0:40576dfac535 5 #include "motor.h"
jford38 0:40576dfac535 6 #include "pen.h"
jford38 0:40576dfac535 7
jford38 0:40576dfac535 8 #define PI 3.1415926535
jford38 0:40576dfac535 9 #define DEG_PER_STEP 1.8
jford38 1:ad895d72e9ed 10 #define INCHES_PER_SEG .25
jford38 0:40576dfac535 11 #define START_Y 10
jford38 0:40576dfac535 12
jford38 1:ad895d72e9ed 13 #define CW_ARC (1)
jford38 1:ad895d72e9ed 14 #define CCW_ARC (-1)
jford38 1:ad895d72e9ed 15
jford38 0:40576dfac535 16 Serial pc(USBTX, USBRX);
jford38 0:40576dfac535 17
jford38 0:40576dfac535 18 class DrawBot {
jford38 0:40576dfac535 19
jford38 0:40576dfac535 20 public:
jford38 0:40576dfac535 21 DrawBot(Motor*, Motor*, PinName, float, float);
jford38 0:40576dfac535 22 ~DrawBot();
jford38 0:40576dfac535 23 void drawCrap();
jford38 0:40576dfac535 24 void line_safe(float x, float y);
jford38 1:ad895d72e9ed 25 void arc(float cx,float cy,float x,float y,float dir);
jford38 0:40576dfac535 26 inline void pen_up();
jford38 0:40576dfac535 27 inline void pen_down();
jford38 0:40576dfac535 28 inline void disable();
jford38 0:40576dfac535 29
jford38 0:40576dfac535 30 private:
jford38 0:40576dfac535 31 float px, py; // Position
jford38 0:40576dfac535 32 float fr; // Feed rate
jford38 0:40576dfac535 33 float step_delay; // How long to delay between steps.
jford38 0:40576dfac535 34 float rad; // Pulley Radius (inches)
jford38 0:40576dfac535 35 float STEP_PER_INCH;
jford38 0:40576dfac535 36 float width; // Distance from Pulley to Pulley (inches)
jford38 0:40576dfac535 37
jford38 0:40576dfac535 38 Motor* mL;
jford38 0:40576dfac535 39 Motor* mR;
jford38 0:40576dfac535 40
jford38 0:40576dfac535 41 Pen* pen;
jford38 0:40576dfac535 42
jford38 0:40576dfac535 43 inline void pause(long ms);
jford38 0:40576dfac535 44 inline void IK(float x, float y, long &l1, long &l2);
jford38 0:40576dfac535 45 void line(float newx,float newy);
jford38 1:ad895d72e9ed 46 float atan3(float dy, float dx);
jford38 0:40576dfac535 47 };
jford38 0:40576dfac535 48
jford38 0:40576dfac535 49 DrawBot::DrawBot(Motor* mL, Motor* mR, PinName s_pin, float rad, float width) {
jford38 0:40576dfac535 50
jford38 0:40576dfac535 51 this->mL = mL;
jford38 0:40576dfac535 52 this->mR = mR;
jford38 0:40576dfac535 53 pen = new Pen(s_pin, 90);
jford38 0:40576dfac535 54
jford38 0:40576dfac535 55 STEP_PER_INCH = mL->get_msLevel()*360.0/(DEG_PER_STEP * 2*PI * rad);
jford38 0:40576dfac535 56 this->rad = rad;
jford38 0:40576dfac535 57 this->width = width;
jford38 0:40576dfac535 58 px = width/2.0;
jford38 0:40576dfac535 59 py = START_Y;
jford38 1:ad895d72e9ed 60 step_delay = 240;
jford38 0:40576dfac535 61 }
jford38 0:40576dfac535 62
jford38 0:40576dfac535 63 DrawBot::~DrawBot() {
jford38 0:40576dfac535 64 delete mL;
jford38 0:40576dfac535 65 delete mR;
jford38 0:40576dfac535 66 }
jford38 0:40576dfac535 67
jford38 0:40576dfac535 68 inline void DrawBot::pause(long us) {
jford38 0:40576dfac535 69 wait_ms(us/1000);
jford38 0:40576dfac535 70 wait_us(us%1000);
jford38 0:40576dfac535 71 }
jford38 0:40576dfac535 72
jford38 0:40576dfac535 73 inline void DrawBot::pen_up() {
jford38 0:40576dfac535 74 pen->up();
jford38 0:40576dfac535 75 }
jford38 0:40576dfac535 76
jford38 0:40576dfac535 77 inline void DrawBot::pen_down() {
jford38 0:40576dfac535 78 pen->down();
jford38 0:40576dfac535 79 }
jford38 0:40576dfac535 80
jford38 0:40576dfac535 81 inline void DrawBot::disable() {
jford38 0:40576dfac535 82 pen->up();
jford38 0:40576dfac535 83 mL->disable();
jford38 0:40576dfac535 84 mR->disable();
jford38 0:40576dfac535 85 }
jford38 0:40576dfac535 86
jford38 0:40576dfac535 87 inline void DrawBot::IK(float x, float y, long &l1, long &l2) {
jford38 0:40576dfac535 88 long w = width * STEP_PER_INCH;
jford38 0:40576dfac535 89 l1 = sqrt(x*x + y*y - rad*rad);
jford38 0:40576dfac535 90 l2 = sqrt(y*y + (w-x)*(w-x) - rad*rad);
jford38 0:40576dfac535 91 }
jford38 0:40576dfac535 92
jford38 0:40576dfac535 93 void DrawBot::line(float newx, float newy) {
jford38 0:40576dfac535 94
jford38 0:40576dfac535 95 pc.printf("LINE: (%f, %f) -> (%f, %f)\n",px,py,newx,newy);
jford38 0:40576dfac535 96
jford38 0:40576dfac535 97 long L1, L2, oldL1, oldL2;
jford38 0:40576dfac535 98 IK(px*STEP_PER_INCH, py*STEP_PER_INCH, oldL1, oldL2);
jford38 0:40576dfac535 99 IK(newx*STEP_PER_INCH, newy*STEP_PER_INCH, L1, L2);
jford38 0:40576dfac535 100
jford38 0:40576dfac535 101 long d1=L1-oldL1;
jford38 0:40576dfac535 102 long d2=L2-oldL2;
jford38 0:40576dfac535 103 int dir1=d1>0?1:0;
jford38 0:40576dfac535 104 int dir2=d2>0?1:0;
jford38 0:40576dfac535 105 d1=abs(d1);
jford38 0:40576dfac535 106 d2=abs(d2);
jford38 0:40576dfac535 107
jford38 0:40576dfac535 108 long i;
jford38 0:40576dfac535 109 long over = 0;
jford38 0:40576dfac535 110
jford38 0:40576dfac535 111 if(d1>d2) {
jford38 0:40576dfac535 112 for(i=0;i<d1;++i) {
jford38 0:40576dfac535 113 mL->one_step(dir1);
jford38 0:40576dfac535 114 over+=d2;
jford38 0:40576dfac535 115 if(over>=d1) {
jford38 0:40576dfac535 116 over-=d1;
jford38 0:40576dfac535 117 mR->one_step(dir2);
jford38 0:40576dfac535 118 }
jford38 0:40576dfac535 119 pause(step_delay);
jford38 0:40576dfac535 120 }
jford38 0:40576dfac535 121 } else {
jford38 0:40576dfac535 122 for(i=0;i<d2;++i) {
jford38 0:40576dfac535 123 mR->one_step(dir2);
jford38 0:40576dfac535 124 over+=d1;
jford38 0:40576dfac535 125 if(over>=d2) {
jford38 0:40576dfac535 126 over-=d2;
jford38 0:40576dfac535 127 mL->one_step(dir1);
jford38 0:40576dfac535 128 }
jford38 0:40576dfac535 129 pause(step_delay);
jford38 0:40576dfac535 130 }
jford38 0:40576dfac535 131 }
jford38 0:40576dfac535 132
jford38 0:40576dfac535 133 px=newx;
jford38 0:40576dfac535 134 py=newy;
jford38 0:40576dfac535 135 }
jford38 0:40576dfac535 136
jford38 0:40576dfac535 137 void DrawBot::line_safe(float x,float y) {
jford38 0:40576dfac535 138
jford38 0:40576dfac535 139 // split up long lines to make them straighter?
jford38 0:40576dfac535 140 float dx=x-px;
jford38 0:40576dfac535 141 float dy=y-py;
jford38 0:40576dfac535 142
jford38 0:40576dfac535 143 float len=sqrt(dx*dx+dy*dy);
jford38 0:40576dfac535 144
jford38 0:40576dfac535 145 if(len<=INCHES_PER_SEG) {
jford38 0:40576dfac535 146 line(x,y);
jford38 0:40576dfac535 147 return;
jford38 0:40576dfac535 148 }
jford38 0:40576dfac535 149
jford38 0:40576dfac535 150 // too long!
jford38 0:40576dfac535 151 long pieces=floor(len/INCHES_PER_SEG);
jford38 0:40576dfac535 152 float x0=px;
jford38 0:40576dfac535 153 float y0=py;
jford38 0:40576dfac535 154
jford38 0:40576dfac535 155 float a;
jford38 0:40576dfac535 156 for(long j=0;j<=pieces;++j) {
jford38 0:40576dfac535 157 a=(float)j/(float)pieces;
jford38 0:40576dfac535 158
jford38 0:40576dfac535 159 line((x-x0)*a+x0,
jford38 0:40576dfac535 160 (y-y0)*a+y0);
jford38 0:40576dfac535 161 }
jford38 0:40576dfac535 162 line(x,y);
jford38 0:40576dfac535 163 }
jford38 0:40576dfac535 164
jford38 0:40576dfac535 165 void DrawBot::drawCrap() {
jford38 0:40576dfac535 166 pc.printf("\nSTARTING\n");
jford38 0:40576dfac535 167 line_safe(width/2.0+4, START_Y);
jford38 0:40576dfac535 168 line_safe(width/2.0+4, START_Y+4);
jford38 0:40576dfac535 169 line_safe(width/2.0, START_Y+4);
jford38 0:40576dfac535 170 line_safe(width/2.0, START_Y);
jford38 0:40576dfac535 171
jford38 0:40576dfac535 172 mL->disable();
jford38 0:40576dfac535 173 mR->disable();
jford38 0:40576dfac535 174 }
jford38 1:ad895d72e9ed 175
jford38 1:ad895d72e9ed 176 //------------------------------------------------------------------------------
jford38 1:ad895d72e9ed 177 // returns angle of dy/dx as a value from 0...2PI
jford38 1:ad895d72e9ed 178 float DrawBot::atan3(float dy,float dx) {
jford38 1:ad895d72e9ed 179 float a=atan2(dy,dx);
jford38 1:ad895d72e9ed 180 if(a<0) a=(PI*2.0)+a;
jford38 1:ad895d72e9ed 181 return a;
jford38 1:ad895d72e9ed 182 }
jford38 1:ad895d72e9ed 183
jford38 0:40576dfac535 184 //------------------------------------------------------------------------------
jford38 0:40576dfac535 185 // This method assumes the limits have already been checked.
jford38 0:40576dfac535 186 // This method assumes the start and end radius match.
jford38 0:40576dfac535 187 // This method assumes arcs are not >180 degrees (PI radians)
jford38 0:40576dfac535 188 // cx/cy - center of circle
jford38 0:40576dfac535 189 // x/y - end position
jford38 0:40576dfac535 190 // dir - ARC_CW or ARC_CCW to control direction of arc
jford38 1:ad895d72e9ed 191 void DrawBot::arc(float cx,float cy,float x,float y,float dir) {
jford38 1:ad895d72e9ed 192
jford38 1:ad895d72e9ed 193 cx += px;
jford38 1:ad895d72e9ed 194 cy += py;
jford38 1:ad895d72e9ed 195
jford38 0:40576dfac535 196 // get radius
jford38 1:ad895d72e9ed 197 float dx = px - cx;
jford38 1:ad895d72e9ed 198 float dy = py - cy;
jford38 0:40576dfac535 199 float radius=sqrt(dx*dx+dy*dy);
jford38 0:40576dfac535 200
jford38 0:40576dfac535 201 // find angle of arc (sweep)
jford38 0:40576dfac535 202 float angle1=atan3(dy,dx);
jford38 0:40576dfac535 203 float angle2=atan3(y-cy,x-cx);
jford38 0:40576dfac535 204 float theta=angle2-angle1;
jford38 0:40576dfac535 205
jford38 0:40576dfac535 206 if(dir>0 && theta<0) angle2+=2*PI;
jford38 0:40576dfac535 207 else if(dir<0 && theta>0) angle1+=2*PI;
jford38 0:40576dfac535 208
jford38 0:40576dfac535 209 theta=angle2-angle1;
jford38 0:40576dfac535 210
jford38 0:40576dfac535 211 // get length of arc
jford38 0:40576dfac535 212 // float circ=PI*2.0*radius;
jford38 0:40576dfac535 213 // float len=theta*circ/(PI*2.0);
jford38 0:40576dfac535 214 // simplifies to
jford38 0:40576dfac535 215 float len = abs(theta) * radius;
jford38 0:40576dfac535 216
jford38 1:ad895d72e9ed 217 int i, segments = floor( len / INCHES_PER_SEG );
jford38 0:40576dfac535 218
jford38 1:ad895d72e9ed 219 float nx, ny, angle3, scale;
jford38 0:40576dfac535 220
jford38 0:40576dfac535 221 for(i=0;i<segments;++i) {
jford38 0:40576dfac535 222 // interpolate around the arc
jford38 0:40576dfac535 223 scale = ((float)i)/((float)segments);
jford38 0:40576dfac535 224
jford38 0:40576dfac535 225 angle3 = ( theta * scale ) + angle1;
jford38 0:40576dfac535 226 nx = cx + cos(angle3) * radius;
jford38 0:40576dfac535 227 ny = cy + sin(angle3) * radius;
jford38 0:40576dfac535 228 // send it to the planner
jford38 1:ad895d72e9ed 229 line(nx,ny);
jford38 0:40576dfac535 230 }
jford38 0:40576dfac535 231
jford38 1:ad895d72e9ed 232 line(x,y);
jford38 1:ad895d72e9ed 233 }
jford38 0:40576dfac535 234
jford38 0:40576dfac535 235 #endif