Jordan Ford / Mbed 2 deprecated DrawBot

Final code for our 4180 Drawing Robot!

Dependencies:   4DGL-uLCD-SE gCodeParser mbed

drawBot.h@0:40576dfac535, 2014-04-29 (annotated)

Committer:
jford38
Date:
Tue Apr 29 21:51:29 2014 +0000
Revision:
0:40576dfac535
Child:
1:ad895d72e9ed
Before I fuck all this up.

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