Hello
I am doing a vertical plotter with two tb6600 drivers. It's using the serial to pass the coordinates. I hope i am not overwhelming with so many questions.
I tried to do this project with raspberry and at the end i saw that it will be much better with Arduino. I am newbie.
In the arduino uno code the pins for the tb6600 are defined
#define DirY 7
#define StepY 4
#define EnableY 8
#define DirX 12
#define StepX 9
#define EnableX 6
Enable and Dir are for ENA and DIR, Does Step goes to PUL?
in that case is that right?
ENA- pin 8, PUL- pin 4 ,DIR- pin 7
ENA+,PUL+ and DIR+ go to 5v pin, no?
I am confuse because of this funcion?
digitalWrite(DirX, HIGH);digitalWrite(StepX, LOW);digitalWrite(EnableX,HIGH);
Is the servo motor connected to pin 3?
#include <Servo.h>
...
Servo servoPen;
.....
servoPen.attach(3);
I am attaching a png file with how i am guessing the schematics are.
and the whole code. Thanks in advantage for any help.
#include <Servo.h>
long i=0;
Servo servoPen;
int PosServoUp = 66;//105
int PosServoDown = 54; // 30
float curX,curY,curZ;
float tarX,tarY,tarZ; // target xyz position
float delayy=80;
float AB=635;
float CO=490;
float L0=sqrt((AB/2)*(AB/2)+CO*CO);
float LA_precedente=L0;
float LB_precedente=L0;
float LA,LB,dlA,dlB;
long StepsA,StepsB;
long CbStepsA,CbStepsB;
/******** mapping xy position to steps ******/
float STEPS_PER_CIRCLE = 3200;
float DIAMETER = 13.50;
//float STEPS_PER_MM = (STEPS_PER_CIRCLE/PI/DIAMETER);
#define DirY 7
#define StepY 4
#define EnableY 8
#define DirX 12
#define StepX 9
#define EnableX 6
void setup(){
Serial.begin(115200);
servoPen.attach(3);
pinMode(DirX, OUTPUT);pinMode(StepX,OUTPUT);pinMode(EnableX, OUTPUT);
pinMode(DirY, OUTPUT);pinMode(StepY,OUTPUT);pinMode(EnableY, OUTPUT);
digitalWrite(DirX, HIGH);digitalWrite(StepX, LOW);digitalWrite(EnableX,HIGH);
digitalWrite(DirY, HIGH);digitalWrite(StepY, LOW);digitalWrite(EnableY,HIGH);
servoPen.write(PosServoUp);
digitalWrite(EnableX,HIGH);
digitalWrite(EnableY,HIGH);
}
void prepareMove()
{
//float dx = tarX - curX;
//float dy = tarY - curY;
//float distance = sqrt(dx*dx+dy*dy);
// map xy to string length
LA=sqrt((AB/2+tarX)*(AB/2+tarX)+(CO-tarY)*(CO-tarY));
LB=sqrt((AB/2-tarX)*(AB/2-tarX)+(CO-tarY)*(CO-tarY));
dlA=LA-LA_precedente;
dlB=LB-LB_precedente; //en mm
ActivDir(dlA,dlB);
StepsA=(3200.00*dlA)/(PI*DIAMETER);
StepsB=(3200.00*dlB)/(PI*DIAMETER);
Rotation(StepsA,StepsB);
//Serial.print((3200*dlA)/(PI*DIAMETER));Serial.print(";");Serial.println((3200*dlB)/(PI*DIAMETER));
LA_precedente=LA_precedente+(PI*DIAMETER*float(CbStepsA))/3200.00;
LB_precedente=LB_precedente+(PI*DIAMETER*float(CbStepsB))/3200.00;
CbStepsA=0;
CbStepsB=0;
//curX = tarX;
//curY = tarY;
//Serial.print(curX);Serial.print(";");Serial.println(curY);
}
void parseCordinate(char * cmd)
{
char * tmp;
char * str;
str = strtok_r(cmd, " ", &tmp);
while(str!=NULL){
str = strtok_r(0, " ", &tmp);
//Serial.printf("%s;",str);
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
}else if(str[0]=='A'){
//stepAuxDelay = atol(str+1);
}
}
prepareMove();
}
void parsePen(char * cmd)
{
char * tmp;
char * str;
str = strtok_r(cmd, " ", &tmp);
int pos = atoi(tmp);
servoPen.write(pos);
}
void parseMcode(char * cmd)
{
int code;
code = atoi(cmd);
switch(code){
case 1:
parsePen(cmd);
break;
}
}
void parseGcode(char * cmd)
{
int code;
code = atoi(cmd);
switch(code){
case 1: // xyz move
parseCordinate(cmd);
break;
case 28: // home
tarX=0; tarY=0;
prepareMove();
break;
}
}
void parseCmd(char * cmd)
{
if(cmd[0]=='G'){ // gcode
parseGcode(cmd+1);
Serial.println("OK");
}else if(cmd[0]=='M'){ // mcode
parseMcode(cmd+1);
Serial.println("OK");
}
}
char buf[64];
char bufindex;
void loop(){
if(Serial.available()){
char c = Serial.read();
//Serial.print(c);
buf[bufindex++]=c;
if(c=='\n'){
parseCmd(buf);
memset(buf,0,64);
bufindex = 0;
}
}
}
void Rotation(long x,long y)
{
float rx,ry,ax,ay;
long m=max(abs(x),abs(y));
ax=0;
ay=0;
rx=abs(float(x))/float(m);
ry=abs(float(y))/float(m);
i=0;
do{
ax=ax+rx;
ay=ay+ry;
if(ax>=1){ax--;digitalWrite(StepX,HIGH);digitalWrite(StepX,LOW);if(digitalRead(DirX)==1){CbStepsA=CbStepsA-1;}else{CbStepsA=CbStepsA+1;}}
if(ay>=1){ay--;digitalWrite(StepY,HIGH);digitalWrite(StepY,LOW);if(digitalRead(DirY)==1){CbStepsB=CbStepsB+1;}else{CbStepsB=CbStepsB-1;}}
i++;
delayMicroseconds(delayy);
}while(i<m); // StopCond
/*Serial.print("Steps A : ");
Serial.println(CbStepsA);
Serial.print("Steps B : ");
Serial.println(CbStepsB);*/
}
void ActivDir(float x,float y)
{
if (x<0)digitalWrite(DirX, HIGH);
else digitalWrite(DirX, LOW);
if (y<0)digitalWrite(DirY, LOW);
else digitalWrite(DirY, HIGH);
}
