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?
User | Revision | Line number | New 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 |