某科技公司正在开发一个3D摄像头,光学镜头的校准基本是人工操作。
于是为了降低劳动强度,他们用Makeblock的机械零件和Zero PI主控板做了一个3D摄像头自动光学校正机器人

整个系统由XYZ三轴运动结构组成,每个轴使用了一个步进电机控制运动。ZeroPi支持4路步进电机,可使用代码控制步进细分和使能等(自制3D打印机必备主控板)众筹链接:http://kck.st/1FxJxLa

IMG_0190.JPG
IMG_0191.JPG
IMG_0192.JPG
IMG_0193.JPG
IMG_0194.JPG


三个轴的运动范围是1米x1米x1米
使用G代码控制:G1 X100 Y100 Z100 F900 - XYZ平移到100mm处,速度900
G28 - 回到零点
XYZ运动的控制代码

[C++] 纯文本查看 复制代码
#include "ZeroPi.h"
#include <Servo.h>
ZeroPiStepper stepperX(SLOT4);
ZeroPiStepper stepperY(SLOT3);
ZeroPiStepper stepperZ(SLOT2);

// arduino only handle A,B,C step mapping
double curSpd,tarSpd; // speed profile
double curX,curY,curZ;
double tarX,tarY,tarZ; // target xyz position
// step value
int32_t tarA,tarB,tarC,posA,posB,posC; // target stepper position
int8_t motorAfw,motorAbk;
int8_t motorBfw,motorBbk;
int8_t motorCfw,motorCbk;
int xlimit_pin1 = A2;
int xlimit_pin2 = A4;//xlimit.pin2();
int ylimit_pin1 = A3;
int ylimit_pin2 = A4;//ylimit.pin1();
int zlimit_pin1 = A1;
int zlimit_pin2 = A4;//
Servo servo;
void setup() {
  // put your setup code here, to run once:
  SerialUSB.begin(115200);
  
  pinMode(xlimit_pin1,INPUT_PULLUP);
  pinMode(xlimit_pin2,INPUT_PULLUP);
  pinMode(ylimit_pin1,INPUT_PULLUP);
  pinMode(ylimit_pin2,INPUT_PULLUP);
  pinMode(zlimit_pin1,INPUT_PULLUP);
  pinMode(zlimit_pin2,INPUT_PULLUP);
  
  initRobotSetup();
  initPosition();
  
  
  stepperX.setResolution(16,DRV8825);
  stepperY.setResolution(16,DRV8825);
  stepperZ.setResolution(16,DRV8825);
  
  stepperX.enable();
  stepperY.enable();
  stepperZ.enable();
  
  servo.attach(A0);
  servo.write(90);
}
char buf[64];
int8_t bufindex;

void loop() {
  if(SerialUSB.available()>0){
    char c = SerialUSB.read();
   if(c>=0x6B){
    SerialUSB.println("ERR");
    bufindex = 0;
    while(SerialUSB.available()){
      SerialUSB.read();
    }
    return;
   }
    buf[bufindex++]=c; 
    if(c=='\n'){
      buf[bufindex-1]='\0';      // Robbo1 2015/6/8 Add     - Null terminate the string - Essential for first use of 'buf' and good programming practice
      SerialUSB.println("ACK");
      parseCmd(buf);
      memset(buf,0,64);
      bufindex = 0;
    }
    if(bufindex>=64){
      bufindex=0;
    }
  }
}
void stepperMoveA(int dir)
{
  stepperX.setDirection(dir==1?0:1);
  stepperX.step();
}
void stepperMoveB(int dir)
{
  stepperY.setDirection(dir==1?0:1);
  stepperY.step();
}
void stepperMoveC(int dir)
{
  stepperZ.setDirection(dir==1?0:1);
  stepperZ.step();
}
int stepAuxDelay=500;
int stepdelay_min=1;
int stepdelay_max=200;
#define ACCELERATION 2 // mm/s^2 don't get inertia exceed motor could handle
#define SEGMENT_DISTANCE 10 // 1 mm for each segment
#define SPEED_STEP 1

void doMove()
{
  int mDelay=stepdelay_min;
  int speedDiff = -SPEED_STEP;
  int32_t dA,dB,dC,maxD;
  float stepA,stepB,stepC,cntA=0,cntB=0,cntC=0;
  int d;
  dA = tarA - posA;
  dB = tarB - posB;
  dC = tarC - posC;
  maxD = max(abs(dC),max(abs(dA),abs(dB)));
  stepA = (float)abs(dA)/(float)maxD;
  stepB = (float)abs(dB)/(float)maxD;
  stepC = (float)abs(dC)/(float)maxD;
//  Serial.print("move: max:%d da:%d db:%d\n",maxD,dA,dB);
//  Serial.print(stepA);Serial.print(' ');Serial.println(stepB);
  for(int i=0;(posA!=tarA)||(posB!=tarB)||(posC!=tarC);i++){                         // Robbo1 2015/6/8 Changed - change loop terminate test to test for moving not finished rather than a preset amount of moves
    //Serial.printf("step %d A:%d B;%d tar:%d %d\n",i,posA,posB,tarA,tarB);
    // move A
    if(posA!=tarA){
      cntA+=stepA;
      if(cntA>=1){
        d = dA>0?motorAfw:motorAbk;
        posA+=(dA>0?1:-1);
        stepperMoveA(d);
        cntA-=1;
      }
    }
    // move B
    if(posB!=tarB){
      cntB+=stepB;
      if(cntB>=1){
        d = dB>0?motorBfw:motorBbk;
        posB+=(dB>0?1:-1);
        stepperMoveB(d);
        cntB-=1;
      }
    }
    // move C
    if(posC!=tarC){
      cntC+=stepC;
      if(cntC>=1){
        d = dC>0?motorCfw:motorCbk;
        posC+=(dC>0?1:-1);
        stepperMoveC(d);
        cntC-=1;
      }
    }
    mDelay=constrain(stepdelay_min+speedDiff/2,stepdelay_min,stepdelay_max);//+stepAuxDelay;
    delayMicroseconds(mDelay);
    if((maxD-i)<((stepdelay_max-stepdelay_min)/SPEED_STEP)){
      speedDiff=SPEED_STEP;
    }
  }
  //Serial.printf("finally %d A:%d B;%d\n",maxD,posA,posB);
  posA = tarA;
  posB = tarB;
  posC = tarC;
}
/******** mapping xy position to steps ******/
#define STEPS_PER_CIRCLE 3200.0f
#define WIDTH 800
#define HEIGHT 800
#define DIAMETER 11 // the diameter of stepper wheel
//#define STEPS_PER_MM (STEPS_PER_CIRCLE/PI/DIAMETER) 
#define STEPS_PER_MM 87.58 // the same as 3d printer
void prepareMove()
{
  float dx = tarX - curX;
  float dy = tarY - curY;
  float dz = tarZ - curZ;
  float distance = sqrt(dx*dx+dy*dy+dz*dz);
  //Serial.print("distance=");Serial.println(distance);
  if (distance < 0.001)
    return;
  tarA = tarX*STEPS_PER_MM;
  tarB = tarY*STEPS_PER_MM;
  tarC = tarZ*STEPS_PER_MM;
  //Serial.print("tarL:");Serial.print(tarL);Serial.print(' ');Serial.print("tarR:");Serial.println(tarR);
  //Serial.print("curL:");Serial.print(curL);Serial.print(' ');Serial.print("curR:");Serial.println(curR);
  //Serial.printf("tar Pos %ld %ld\r\n",tarA,tarB);
  doMove();
  curX = tarX;
  curY = tarY;
  curZ = tarZ;
  
}

void goHome()
{
  // stop on either endstop touches
  stepdelay_min = 100;
//  Serial.print("stepdelay_min:");
//  Serial.println(stepdelay_min);
  while(digitalRead(xlimit_pin1)==1 && digitalRead(xlimit_pin1)==1){
    stepperMoveB(motorBbk);
    delayMicroseconds(stepdelay_min);
  }
  while(digitalRead(ylimit_pin1)==1 && digitalRead(ylimit_pin1)==1){
    stepperMoveA(motorAbk);
    delayMicroseconds(stepdelay_min);
  }
  while(digitalRead(zlimit_pin1)==1 && digitalRead(zlimit_pin1)==1){
    stepperMoveC(motorCbk);
    delayMicroseconds(stepdelay_min);
  }
  posA = 0;
  posB = 0;
  posC = 0;
  curX = 0;
  curY = 0;
  curZ = 0;
}

void initPosition()
{
  curX=0; curY=0; curZ=0;
  posA=0; posB=0; posC=9;
}

/************** calculate movements ******************/
void parseCordinate(char * cmd)
{
  char * tmp;
  char * str;
  str = strtok_r(cmd, " ", &tmp);
  tarX = curX;
  tarY = curY;
  tarZ = curZ;
  while(str!=NULL){
    str = strtok_r(0, " ", &tmp);
    if(str[0]=='X'){
      tarX = atof(str+1);
    }else if(str[0]=='Y'){
      tarY = atof(str+1);
    }else if(str[0]=='Z'){
      tarZ = atof(str+1);
    }else if(str[0]=='F'){
      float speed = atof(str+1);
      tarSpd = speed/60; // mm/min -> mm/s
      stepdelay_min = 50+(2000-speed*2);
    }else if(str[0]=='A'){
      stepAuxDelay = atoi(str+1);
    }
  }
  
  prepareMove();
}



void parseAuxDelay(char * cmd)
{
  char * tmp;
  strtok_r(cmd, " ", &tmp);
  stepAuxDelay = atoi(tmp);
}

void parseServo(char * cmd)
{
  char * tmp;
  char * str;
  str = strtok_r(cmd, " ", &tmp);
  while(str!=NULL){
    str = strtok_r(0, " ", &tmp);
    if(str[0]=='P'){
      int angle = atoi(str+1);
      if(angle<10){
        angle = 10;
      }
      if(angle>170){
        angle = 170;
      }
      servo.write(angle);
    };
  }
}

void parseMcode(char * cmd)
{
  int code;
  code = atoi(cmd);
  switch(code){
    case 1:
      break;
    case 2: // set pen position
      break;
    case 3:
      parseServo(cmd);
      break;
    case 4:
      break;
    case 5:
      break;  
    case 6:
      break;    
  }
}

void parseGcode(char * cmd)
{
  int code;
  code = atoi(cmd);
  switch(code){
    case 0:
    case 1: // xyz move
      parseCordinate(cmd);
      break;
    case 28: // home
      tarX=0; tarY=0;
      goHome();
      break; 
  }
}

void parseCmd(char * cmd)
{
  if(cmd[0]=='G'){ // gcode
    parseGcode(cmd+1);  
  }else if(cmd[0]=='M'){ // mcode
    parseMcode(cmd+1);
  }else if(cmd[0]=='P'){
//    SerialUSB.print("POS X");SerialUSB.print(curX);SerialUSB.print(" Y");SerialUSB.println(curY);
  }
  SerialUSB.println("OK");
}

// local data
void initRobotSetup()
{
  motorAfw=-1;motorAbk=1;
  motorBfw=-1;motorBbk=1;
  motorCfw=1;motorCbk=-1;
  int spd = 50;
  stepdelay_min = spd*10;
  stepdelay_max = spd*100;
}






XYZBot.zip

7.22 KB, 下载次数: 841

收藏
1 条回帖
大连林海论坛元老2015-10-11 16:13:48
酷酷的 作品
需要登陆后才可进行回复 登录

返回顶部
现在加入我们,注册一个账号 账号登陆 QQ账号登陆 微博账号登陆