arduino开源项目——墙画机器人

由于有的项目原作者限制了转载和引用，所以不便于公开整个制作过程，而这个项目是个开源的项目，所以可以当做学习笔记来记录。

墙画机原理

通过两个步进电机控制线轴转动，放线和收线来控制笔的移动位置，将这种位置转换成坐标，通过坐标定位放线和收线来绘制想要的图形。

定位方法原理

三角函数

刚看的时候发现忘完了三角函数，于是看了下基本概念，并做了简单的推导。

image.png

1.求C的坐标（C为笔的位置）
AC长度=b,CB长度=a,AB长度=c，A/B对称于y坐标，CD为辅助线，垂直于AB。
解：设AD长为x，则BD为c-x
根据勾股定理：
bb-xx=aa-(c-x)(c-x)
得：
bb-xx=aa-(cc-2cx+xx)
bb-xx=aa-cc+2cx-xx
bb=aa-cc+2cx
x=(bb+cc-aa)/2c
那么：
cos(A)=x/b=(bb+cc-aa)/2cb（余弦函数）
设C坐标（X,Y）
那么：
X=cos(a)b-A0
Y=sin(a)*b-D0
已知AB为墙画机的两个轮盘的位置，以及轮盘距离中心点位置都是可以测量出来的，那么笔的位置坐标就确定下来了。
2.已知C的坐标，求两条绳子的长度，也就是b和a的长度。
相当于上面操作的逆运算。
b=√(X+A0)^2+(Y-D0)2（注意XY坐标可能为负数）
a=√(X-A0)^2+(Y-D0)2
简单的勾股定理构成了墙画机的核心思路，通过收线放线控制a、b的长度，来确定坐标，通过坐标转换a、b来确定收线放线的长度，来进行绘画操作。

收线、放线规则

假如线的长度为无线长，那么不需要考虑两个步进电机的转动收线放线。
1.每根线长度收线不能小于0。
2.每根线的放线不能大于每根线的长度，否则会反向卷线。
3.两线相加的长度不能少于AB的距离。

步数转化

已知步进电机2048步完成1周转动（360度），线轴（轮盘）直径35毫米，那么线轴的周长为35*π，步进电机一步的长度为35π/2048。
那么就能将坐标转化成步进电机的步数了。

抬笔落笔

通过舵机简单控制。

• (函数式编程和面向对象编程思路总觉得有点反逻辑。普遍代码是根据现有正向推导的顺序结构。而函数和面向对象总有种逆向思维的感觉。所以看到源码总是很难理解，很有可能作者是边写边封装，所以解读代码的时候，最重要的是抓住主干，否则很有可能陷入海量的变量和想的头疼都不得要领的境地。)

#include <TinyStepper_28BYJ_48.h>
#include <Servo.h>

#include <SD.h>  //需要SD卡读卡器模块，或者tf读卡器模块



//调试代码标志，去掉注释，可以输出调试信息（程序运行会慢）
//#define VERBOSE         (1)
//调试标志


#define STEPS_PER_TURN  (2048)  //步进电机一周步长 2048步转360度
#define SPOOL_DIAMETER  (35)    //线轴直径mm
#define SPOOL_CIRC      (SPOOL_DIAMETER * 3.1416)  //线轴周长 35*3.14=109.956
#define TPS             (SPOOL_CIRC / STEPS_PER_TURN)  //步进电机步距，最小分辨率 每步线绳被拉动的距离  0.0268447265625mm



#define step_delay      1   //步进电机每步的等候时间 （微妙）
#define TPD             300   //转弯等待时间（毫秒），由于惯性笔会继续运动，暂定等待笔静止再运动。


//两个电机的旋转方向  1正转  -1反转  
//调节进出方向可垂直反转图像
#define M1_REEL_OUT     1     //放出线
#define M1_REEL_IN      -1      //卷入线
#define M2_REEL_OUT     -1      //放出线
#define M2_REEL_IN      1     //卷入线


static long laststep1, laststep2; //当前线长度 记录笔位置


#define X_SEPARATION  507           //两绳上方的水平距离mm 
#define LIMXMAX       ( X_SEPARATION*0.5)   //x轴最大值  0位在画板中心
#define LIMXMIN       (-X_SEPARATION*0.5)   //x轴最小值

/* 垂直距离的参数： 正值在画板下放，理论上只要画板够大可以无限大，负值区域在笔（开机前）的上方 
详细介绍见说明文档 https://github.com/shihaipeng03/Walldraw
*/
#define LIMYMAX         (-440)   //y轴最大值 画板最下方
#define LIMYMIN         (440)    //y轴最小值 画板最上方  左右两线的固定点到笔的垂直距离，尽量测量摆放准确，误差过大会有畸变
                //值缩小画图变瘦长，值加大画图变矮胖 



//抬笔舵机的角度参数  具体数值要看摆臂的安放位置，需要调节
#define PEN_UP_ANGLE    60  //抬笔
#define PEN_DOWN_ANGLE  95  //落笔
//需要调节的参数 =============================================


#define PEN_DOWN 1  //笔状态  下笔
#define PEN_UP 0    //笔状态  抬笔



struct point { 
  float x; 
  float y; 
  float z; 
};

struct point actuatorPos;


// plotter position 笔位置.
static float posx;
static float posy;
static float posz;  // pen state
static float feed_rate = 0;

// pen state 笔状态（抬笔，落笔）.
static int ps;

/*以下为G代码通讯参数 */
#define BAUD            (115200)    //串口速率，用于传输G代码或调试 可选9600，57600，115200 或其他常用速率
#define MAX_BUF         (64)      //串口缓冲区大小

// Serial comm reception
static int sofar;               // Serial buffer progress

static float mode_scale;   //比例

File myFile;

Servo pen;

TinyStepper_28BYJ_48 m1; //(7,8,9,10);  //M1 L步进电机   in1~4端口对应UNO  7 8 9 10
TinyStepper_28BYJ_48 m2; //(2,3,5,6);  //M2 R步进电机   in1~4端口对应UNO 2 3 5 6





//------------------------------------------------------------------------------
//正向运动计算 - 将L1，L2长度转换为XY坐标
// 使用余弦定律， theta = acos((a*a+b*b-c*c)/(2*a*b));
//找到M1M2和M1P之间的角度，其中P是笔的位置
void FK(float l1, float l2,float &x,float &y) {
  float a=l1 * TPS;
  float b=X_SEPARATION;
  float c=l2 * TPS;
  
   
  
  //方法1
  float theta = acos((a*a+b*b-c*c)/(2.0*a*b));
  x = cos(theta)*l1 + LIMXMIN;
  y = sin(theta)*l1 + LIMYMIN;          

  //方法2
/*   float theta = (a*a+b*b-c*c)/(2.0*a*b);
  x = theta*l1 + LIMXMIN;
  y = sqrt (1.0 - theta * theta ) * l1 + LIMYMIN;*/
}


//------------------------------------------------------------------------------
//反向运动 - 将XY坐标转换为长度L1，L2 
void IK(float x,float y,long &l1, long &l2) {
  float dy = y - LIMYMIN;
  float dx = x - LIMXMIN;
  l1 = round(sqrt(dx*dx+dy*dy) / TPS);
  dx = x - LIMXMAX;
  l2 = round(sqrt(dx*dx+dy*dy) / TPS);
}



//------------------------------------------------------------------------------
//笔状态
void pen_state(int pen_st) {
  if(pen_st==PEN_DOWN) {
        ps=PEN_DOWN_ANGLE;
        // Serial.println("Pen down");
      } else {
        ps=PEN_UP_ANGLE;
        //Serial.println("Pen up");
      }
  pen.write(ps);
}


//
void pen_down()
{
  if (ps==PEN_UP_ANGLE)
  {
    ps=PEN_DOWN_ANGLE;
    pen.write(ps);
    delay(TPD);
  }

}

void pen_up()
{
  if (ps==PEN_DOWN_ANGLE)
  {
    ps=PEN_UP_ANGLE;
    pen.write(ps);
  }

  
}

//------------------------------------------------------------------------------
//调试代码串口输出机器状态
void where() {
  Serial.print("X,Y=  ");
  Serial.print(posx);
  Serial.print(",");
  Serial.print(posy);
  Serial.print("\t");
  Serial.print("Lst1,Lst2=  ");
  Serial.print(laststep1);
  Serial.print(",");
  Serial.println(laststep2);
  Serial.println("");
}



//------------------------------------------------------------------------------
// returns angle of dy/dx as a value from 0...2PI
static float atan3(float dy, float dx) {
  float a = atan2(dy, dx);
  if (a < 0) a = (PI * 2.0) + a;
  return a;
}


//------------------------------------------------------------------------------
//画圆弧
static void arc(float cx, float cy, float x, float y,  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;

  // 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 / TPS);

  float nx, ny, nz, angle3, scale;

  for (i = 0; i < segments; ++i) {

    if (i==0) 
      pen_up();
    else
      pen_down();  
    scale = ((float)i) / ((float)segments);

    angle3 = (theta * scale) + angle1;
    nx = cx + cos(angle3) * radius;
    ny = cy + sin(angle3) * radius;
    // send it to the planner
    line_safe(nx, ny);
  }

  line_safe(x, y);
  pen_up();
}



//------------------------------------------------------------------------------
// instantly move the virtual plotter position
// does not validate if the move is valid
static void teleport(float x, float y) {
  posx = x;
  posy = y;
  long l1,l2;
  IK(posx, posy, l1, l2);
  laststep1 = l1;
  laststep2 = l2;
}


//==========================================================
//参考————斜线程序
void moveto(float x,float y) {
  #ifdef VERBOSE
  Serial.println("Jump in line() function");
  Serial.print("x:");
  Serial.print(x);
  Serial.print(" y:");
  Serial.println(y);
  #endif

  long l1,l2;
  IK(x,y,l1,l2);
  long d1 = l1 - laststep1;
  long d2 = l2 - laststep2;

  #ifdef VERBOSE
  Serial.print("l1:");
  Serial.print(l1);
  Serial.print(" laststep1:");
  Serial.print(laststep1);
  Serial.print(" d1:");
  Serial.println(d1);
  Serial.print("l2:");
  Serial.print(l2);
  Serial.print(" laststep2:");
  Serial.print(laststep2);
  Serial.print(" d2:");
  Serial.println(d2);
  #endif

  long ad1=abs(d1);
  long ad2=abs(d2);
  int dir1=d1>0 ? M1_REEL_IN : M1_REEL_OUT;
  int dir2=d2>0 ? M2_REEL_IN : M2_REEL_OUT;
  long over=0;
  long i;


  if(ad1>ad2) {
    for(i=0;i<ad1;++i) {
      
      m1.moveRelativeInSteps(dir1);
      over+=ad2;
      if(over>=ad1) {
        over-=ad1;
        m2.moveRelativeInSteps(dir2);
      }
      delayMicroseconds(step_delay);
     }
  } 
  else {
    for(i=0;i<ad2;++i) {
      m2.moveRelativeInSteps(dir2);
      over+=ad1;
      if(over>=ad2) {
        over-=ad2;
        m1.moveRelativeInSteps(dir1);
      }
      delayMicroseconds(step_delay);
    }
  }

  laststep1=l1;
  laststep2=l2;
  posx=x;
  posy=y;

  
}

//------------------------------------------------------------------------------
//长距离移动会走圆弧轨迹，所以将长线切割成短线保持直线形态
static void line_safe(float x,float y) {
  // split up long lines to make them straighter?
  float dx=x-posx;
  float dy=y-posy;

  float len=sqrt(dx*dx+dy*dy);
  
  if(len<=TPS) {
    moveto(x,y);
    return;
  }
  
  // too long!
  long pieces=floor(len/TPS);
  float x0=posx;
  float y0=posy;
  float a;
  for(long j=0;j<=pieces;++j) {
    a=(float)j/(float)pieces;

    moveto((x-x0)*a+x0,
         (y-y0)*a+y0);
  }
  moveto(x,y);
}





void line(float x,float y) 
{
  line_safe(x,y);
}



//********************************
void nc(String st)
{

String xx,yy,zz;
int ok=1;


  st.toUpperCase();
  
  float x,y,z;
  int px,py,pz;
  px = st.indexOf('X');
  py = st.indexOf('Y');
  pz = st.indexOf('Z');
  if (px==-1 || py==-1) ok=0; 
  if (pz==-1) 
    {
      pz=st.length();
    }
    else
    {   
      zz = st.substring(pz+1,st.length());
      z  = zz.toFloat();
      if (z>0)  pen_up();
      if (z<=0) pen_down();
    }

  xx = st.substring(px+1,py);
  yy = st.substring(py+1,pz);
  
  
  
  xx.trim();//缩进，去掉末尾空格*/
  yy.trim();

  if (ok) line(xx.toFloat(),yy.toFloat());
  
}

//**********************
void drawfile( String filename)
{

  String rd="";
  int line=0;
  char rr=0;
  

  myFile = SD.open(filename);
  if (myFile) {
    Serial.println("OPEN:");
    
    while (myFile.available()) {
      rr=myFile.read();
      
      if (rr == char(10)) 
       {
          line++;
          Serial.print("Run nc #");
          Serial.print(line);
          Serial.println(" : "+rd);
          nc(rd);
          rd="";
        }
       else
         rd+=rr;
        
    }
    
    myFile.close();
    
  } 

}



void setup() {
  // put your setup code here, to run once:
  Serial.begin(BAUD);
  m1.connectToPins(7,8,9,10); //M1 L步进电机   in1~4端口对应UNO  7 8 9 10
  m2.connectToPins(2,3,5,6);  //M2 R步进电机   in1~4端口对应UNO 2 3 5 6
  m1.setSpeedInStepsPerSecond(10000);
  m1.setAccelerationInStepsPerSecondPerSecond(100000);
  m2.setSpeedInStepsPerSecond(10000);
  m2.setAccelerationInStepsPerSecondPerSecond(100000);


  //抬笔舵机
  pen.attach(A0);
  ps=PEN_UP_ANGLE;
  pen.write(ps);

  //将当前笔位置设置为0，0
  teleport(0, 0);
  //缩放比例
  mode_scale = 1;

  if (!SD.begin(4)) {
    Serial.println("initialization failed!");
    while (1);
  }


  Serial.println("Test OK!");
}




void loop() {
 //注意卡上的文件名要和程序一致。！！！！！
  drawfile("1.nc");  //1.nc 是Gcode代码的文件名 ，需要将g代码保存在sd卡上。
  while(1);
}

arduino需要动手，在程序之外的接线，调试硬件等等。
代码很长，下面分享一下解读的办法：

找到核心的主程序。也就是void step 和 void loop部分。

image.png

void setup部分

①BAUD是个变量指代的是波特率，那么在程序中找到。

image.png

可以用CTRL+F查找。
②③m1,m2指代的是两个步进电机。

image.png
④pen指代的是舵机 image.png
ps image.png
PEN_UP_ANGLE image.png
下一句写入ps也就是抬笔舵机的角度为60。
⑤teleport(0,0)是一个函数并传入两个参数0，0
image.png
⑧posx，posy image.png 第一次运行为0，0因为传入了参数。
⑨ IK(posx, posy, l1, l2);它又是一个函数，程序再次跳转。 image.png
⑩①LIMXMIN,LIMYMIN image.png
⑩②TPS步进电机的步长 image.png
在文章的开始部分已经详细介绍了坐标转换两根线的长度，这段代码就是这个意思。程序运行完转到⑩
image.png
image.png
程序转到主程序⑥继续执行。
mode_scale=1; image.png
下面的： image.png
如果SD的信道不是4那么打印加载失败并进入while(1)死循环中。
Serial.println("Test OK!");串口打印：测试通过

void loop()部分

image.png
drawfile("1.nc");是个函数，程序跳转。 image.png
SD image.png
(为了更好体现程序运行过程，公共变量不再贴图)
drawfile()函数代码的意思为：打开文件并赋值给myFile,

如果myFile为真，则串口打印OPEN，如果myFile的字节数为真（也就是大于0），在while循环中读值，然后串口输出打印什么，然后执行nc()函数，程序跳转。

image.png
nc()函数是将sd卡中的gcode码进行处理，当处理完成后，交给line()函数处理。④
line()直接跳转给line_safe(),line_safe函数的作用是计算坐标点和上一个坐标点的距离，具体的操作和步进电机的运行交给moveto()函数进行具体的收线放线等操作。
image.png
整个程序按流程顺下来，等于熟悉了程序框架，剩下的就是些细节，比如，从sd卡读值和字符串处理过程，步进电机的收线放线等等。