Revision 0:40576dfac535, committed 2014-04-29

Comitter:

jford38

Date:

Tue Apr 29 21:51:29 2014 +0000

Child:

1:ad895d72e9ed

Commit message:

Before I fuck all this up.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/4DGL-uLCD-SE.lib	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/4180_1/code/4DGL-uLCD-SE/#e39a44de229a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drawBot.h	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,220 @@
+#ifndef DRAWBOT_H
+#define DRAWBOT_H
+
+#include "mbed.h"
+#include "motor.h"
+#include "pen.h"
+
+#define PI 3.1415926535
+#define DEG_PER_STEP 1.8
+#define INCHES_PER_SEG 4
+#define START_Y 10
+
+Serial pc(USBTX, USBRX);
+
+class DrawBot {
+    
+    public: 
+        DrawBot(Motor*, Motor*, PinName, float, float);
+        ~DrawBot();
+        void drawCrap();
+        void line_safe(float x, float y);
+        inline void pen_up();
+        inline void pen_down();
+        inline void disable();
+        
+    private:
+        float px, py;       // Position
+        float fr;           // Feed rate
+        float step_delay;   // How long to delay between steps.  
+        float rad;          // Pulley Radius (inches)  
+        float STEP_PER_INCH;
+        float width;        // Distance from Pulley to Pulley (inches)
+        
+        Motor* mL;
+        Motor* mR; 
+        
+        Pen* pen;  
+        
+        inline void pause(long ms); 
+        inline void IK(float x, float y, long &l1, long &l2);   
+        void line(float newx,float newy);
+  
+};
+
+DrawBot::DrawBot(Motor* mL, Motor* mR, PinName s_pin, float rad, float width) {
+    
+    this->mL = mL;
+    this->mR = mR;
+    pen = new Pen(s_pin, 90);
+    
+    STEP_PER_INCH = mL->get_msLevel()*360.0/(DEG_PER_STEP * 2*PI * rad);
+    this->rad = rad;
+    this->width = width;
+    px = width/2.0;
+    py = START_Y;
+    step_delay = 80;
+}
+
+DrawBot::~DrawBot() {
+    delete mL;
+    delete mR;
+}
+
+inline void DrawBot::pause(long us) {
+    wait_ms(us/1000);
+    wait_us(us%1000);    
+}
+
+inline void DrawBot::pen_up() {
+    pen->up();
+}
+
+inline void DrawBot::pen_down() {
+    pen->down();
+}
+
+inline void DrawBot::disable() {
+    pen->up();
+    mL->disable();
+    mR->disable();   
+}
+
+inline void DrawBot::IK(float x, float y, long &l1, long &l2) {
+    long w = width * STEP_PER_INCH;
+    l1 = sqrt(x*x + y*y - rad*rad);
+    l2 = sqrt(y*y + (w-x)*(w-x) - rad*rad);
+}
+
+void DrawBot::line(float newx, float newy) {
+
+    pc.printf("LINE: (%f, %f) -> (%f, %f)\n",px,py,newx,newy);
+
+    long L1, L2, oldL1, oldL2;
+    IK(px*STEP_PER_INCH, py*STEP_PER_INCH, oldL1, oldL2);
+    IK(newx*STEP_PER_INCH, newy*STEP_PER_INCH, L1, L2);
+
+    long d1=L1-oldL1;
+    long d2=L2-oldL2;
+    int dir1=d1>0?1:0;
+    int dir2=d2>0?1:0;  
+    d1=abs(d1);
+    d2=abs(d2);
+    
+    long i;
+    long over = 0;
+    
+    if(d1>d2) {
+        for(i=0;i<d1;++i) {
+          mL->one_step(dir1);
+          over+=d2;
+          if(over>=d1) {
+            over-=d1;
+            mR->one_step(dir2);
+          }
+          pause(step_delay);
+        }
+    } else {
+        for(i=0;i<d2;++i) {
+          mR->one_step(dir2);
+          over+=d1;
+          if(over>=d2) {
+            over-=d2;
+            mL->one_step(dir1);
+          }
+          pause(step_delay);
+        }
+    }
+    
+    px=newx;
+    py=newy;
+}
+
+void DrawBot::line_safe(float x,float y) {
+  
+  // split up long lines to make them straighter?
+  float dx=x-px;
+  float dy=y-py;
+
+  float len=sqrt(dx*dx+dy*dy);
+  
+  if(len<=INCHES_PER_SEG) {
+    line(x,y);
+    return;
+  }
+  
+  // too long!
+  long pieces=floor(len/INCHES_PER_SEG);
+  float x0=px;
+  float y0=py;
+
+  float a;
+  for(long j=0;j<=pieces;++j) {
+    a=(float)j/(float)pieces;
+    
+    line((x-x0)*a+x0,
+         (y-y0)*a+y0);
+  }
+  line(x,y);
+}
+
+void DrawBot::drawCrap() {
+    pc.printf("\nSTARTING\n");
+    line_safe(width/2.0+4, START_Y);
+    line_safe(width/2.0+4, START_Y+4);
+    line_safe(width/2.0, START_Y+4);
+    line_safe(width/2.0, START_Y);
+
+    mL->disable();
+    mR->disable();
+}
+/*
+//------------------------------------------------------------------------------
+// This method assumes the limits have already been checked.
+// This method assumes the start and end radius match.
+// This method assumes arcs are not >180 degrees (PI radians)
+// 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) {
+  // get radius
+  float dx = posx - cx;
+  float dy = posy - cy;
+  float radius=sqrt(dx*dx+dy*dy);
+
+  // find angle of arc (sweep)
+  float angle1=atan3(dy,dx);
+  float angle2=atan3(y-cy,x-cx);
+  float theta=angle2-angle1;
+  
+  if(dir>0 && theta<0) angle2+=2*PI;
+  else if(dir<0 && theta>0) angle1+=2*PI;
+  
+  theta=angle2-angle1;
+  
+  // get length of arc
+  // float circ=PI*2.0*radius;
+  // float len=theta*circ/(PI*2.0);
+  // simplifies to
+  float len = abs(theta) * radius;
+
+  int i, segments = floor( len / CM_PER_SEGMENT );
+ 
+  float nx, ny, nz, angle3, scale;
+
+  for(i=0;i<segments;++i) {
+    // interpolate around the arc
+    scale = ((float)i)/((float)segments);
+    
+    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(x,y,z);
+}*/
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gCodeParser.lib	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ajb88/code/gCodeParser/#7818b02dde4b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,59 @@
+#include "mbed.h"
+#include "gparser.h"
+#include "MODSERIAL.h"
+#include "motor.h"
+#include "drawBot.h"
+#include "uLCD_4DGL.h"
+
+#define CMD_LIST_SIZE 512
+
+MODSERIAL gpc(USBTX, USBRX);
+uLCD_4DGL uLCD(p9, p10, p11);
+//G_cmd gcmd_list[CMD_LIST_SIZE]; // buffer to be filled with commands, main program retrieves commands from here
+
+extern G_cmd* gcmd_list;
+extern list_position;
+
+Motor mL(p29, p30, p26, p27, p28, p25, RIGHT_MOTOR);
+Motor mR(p12, p13, p16, p15, p14, p17, LEFT_MOTOR);
+DrawBot bot(&mL, &mR, p21, 0.5, 45.125);
+
+int main() {
+        
+        parserInit();
+    
+        int list_pos = fillInCmdList();
+        uLCD.printf("%d\r\n",list_pos);
+        
+        for(int c=0; c<list_pos; c++) {
+            G_cmd* next = gcmd_list+c;
+            uLCD.printf("G:%d X%.2f Y%.2f Z%.2f I%.2f J%.2f\n",next->X, next->Y, next->Z, next->I,next->J,next->F);
+            wait(10);
+            uLCD.cls();              
+            switch(next->G) {
+                case 0:
+                case 1:
+                    //Draw a line.
+                    if(next->X != -1 && next->Y != -1) {
+                        bot.line_safe(next->X, next->Y);
+                    }
+                    if(next->Z > 0) {
+                        bot.pen_up();    
+                    }else if(next->Z == 0) {
+                        bot.pen_down();    
+                    }
+                break;
+                case 2:
+                    //Draw an arc or something.
+                    //bot->arc(px+i, py+j, next_cmd->X, next_cmd->Z);
+                break;
+                case 3:
+                    //Draw a different arc or something.
+                break;
+                default:
+                break;
+                
+            }
+        }   
+        bot.disable();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,147 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "mbed.h"
+
+#define LEFT_MOTOR 0
+#define RIGHT_MOTOR 1
+
+class Motor {
+  public: 
+    
+    Motor(PinName s, PinName d, PinName ms0, PinName ms1, PinName ms2, PinName en, char L_R);
+    ~Motor();    
+    
+    inline void one_step();
+    inline void one_step(char new_dir);
+    inline void enable();
+    inline void disable();
+    inline void set_dir(char new_dir);
+    inline void set_msLevel(char newLevel);
+    inline char get_msLevel();
+    
+    enum {
+        MS_ONE = 1,
+        MS_TWO = 2,
+        MS_FOUR = 4,
+        MS_EIGHT = 8,
+        MS_SIXTEEN = 16,
+        MS_THIRTYTWO = 32
+    };
+    
+    
+  private:
+  
+    DigitalOut* step_pin;
+    DigitalOut* dir_pin; 
+    DigitalOut* ms0_pin;
+    DigitalOut* ms1_pin;
+    DigitalOut* ms2_pin;
+    DigitalOut* enable_pin;
+    
+    char IN, OUT;
+    char msLevel;
+    
+};
+
+Motor::Motor(PinName s, PinName d, PinName ms0, PinName ms1, PinName ms2, PinName en, char L_R) {
+    
+    step_pin = new DigitalOut(s);
+    dir_pin  = new DigitalOut(d);
+    ms0_pin  = new DigitalOut(ms0);
+    ms1_pin  = new DigitalOut(ms1);
+    ms2_pin  = new DigitalOut(ms2);
+    enable_pin = new DigitalOut(en);
+    
+    switch(L_R) {
+        case LEFT_MOTOR:
+            IN = 1; OUT = 0;
+        break;
+        case RIGHT_MOTOR:
+            IN = 0; OUT = 1;
+        break;   
+        default:
+            // ERROR
+        break;
+    }
+    
+    set_dir(IN);
+    set_msLevel(MS_THIRTYTWO);
+    enable();
+}
+
+Motor::~Motor() {
+    delete step_pin;
+    delete dir_pin;    
+}
+
+inline void Motor::enable() {
+    *enable_pin = 0;
+}
+
+inline void Motor::disable() {
+    *enable_pin = 1;
+}
+
+
+inline void Motor::set_dir(char new_dir) {
+    *dir_pin = new_dir;
+}
+
+inline void Motor::one_step() {
+    *step_pin = 0;
+    *step_pin = 1;
+    wait_us(6);
+    *step_pin = 0;
+}
+
+inline void Motor::one_step(char new_dir) {
+    set_dir(new_dir);
+    one_step();
+}
+
+inline void Motor::set_msLevel(char newLevel) {
+    
+    int level = MS_ONE;
+    
+    switch(newLevel) {
+        case MS_ONE:
+            level = 0;
+            msLevel = MS_ONE;
+            break;
+        case MS_TWO:
+            level = 1;
+            msLevel = MS_TWO;
+            break;
+        case MS_FOUR:
+            level = 2;
+            msLevel = MS_FOUR;
+            break;
+        case MS_EIGHT:
+            level = 3;
+            msLevel = MS_EIGHT;
+            break;
+        case MS_SIXTEEN:
+            level = 4;
+            msLevel = MS_SIXTEEN;
+            break;
+        case MS_THIRTYTWO:
+            level = 5;
+            msLevel = MS_THIRTYTWO;
+            break;
+        default:
+            // Invalid microstep level!
+            break;
+    }
+    
+    *ms0_pin = level & 0x1;
+    *ms1_pin = (level >> 1) & 0x1;
+    *ms2_pin = (level >> 2) & 0x1;
+
+}
+
+inline char Motor::get_msLevel() {
+    return msLevel;
+}
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pen.h	Tue Apr 29 21:51:29 2014 +0000
@@ -0,0 +1,49 @@
+#ifndef PEN_H
+#define PEN_H
+
+#include "mbed.h"
+
+class Pen {
+    
+    public:
+        Pen(PinName p, float r);
+        ~Pen();
+        
+        void up();
+        void down();
+        void pos(float angle);
+        
+    private:
+        PwmOut* pen_pin;
+        float range;
+        float up_range, down_range;
+    
+};
+
+Pen::Pen(PinName p, float r) {
+    range = r;
+    up_range = 1;
+    down_range = 1+90.0/range;
+    pen_pin = new PwmOut(p);
+    pen_pin->period_ms(20);
+    up();
+}
+
+Pen::~Pen() {
+    delete pen_pin;
+}
+
+// Go to angle in degrees
+inline void Pen::pos(float angle) {
+    pen_pin->pulsewidth_ms(1+angle/range); 
+}
+
+inline void Pen::up() {
+    pen_pin->pulsewidth_ms(up_range); 
+}
+
+inline void Pen::down() {
+    pen_pin->pulsewidth_ms(down_range);
+}
+
+#endif
\ No newline at end of file