Skip to content
Snippets Groups Projects
Commit 5b2a5f0a authored by 42loop's avatar 42loop
Browse files

further adaption to real hardware

parent afb5828b
No related branches found
No related tags found
No related merge requests found
/////#define debug
/////#define info
#define debug
#define info
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
......@@ -15,30 +15,38 @@
#define encZB 29
//switch inputs, clamped to 5V by zener diode
#define limitXM 0
#define limitXP 1
#define refX 2
#define limitYM 3
#define limitYP 4
#define refY 5
#define limitZM 6
#define limitZP 7
#define refZ 8
//ready inputs from servo amplifiers
#define readyX 9
#define readyY 10
#define readyZ 11
//output to reed relais
#define enableX 33
#define enableY 34
#define enableZ 35
//switch inputs leveled by external circuits A & B
#define limitXM 0 //EXT A,1
#define limitXP 1 //EXT A,2
#define refX 2 //EXT B,1
#define limitYM 3 //EXT A,3
#define limitYP 4 //EXT A,4
#define refY 5 //EXT B,3
#define limitZM 6 //EXT A,5
#define limitZP 7 //EXT A,6
#define refZ 8 //EXT B,5
//ready inputs from amps, leveled by external circuit
#define readyX 9 //EXT B,2
#define readyY 10 //EXT B,4
#define readyZ 11 //EXT B,6
//output to 24V, external circuit C
#define enableX 33 //EXT C,1
#define enableY 34 //EXT C,2
#define enableZ 35 //EXT C,3
//brake outputs
#define brakeX 14
#define brakeY 15
#define brakeZ 16
//encoder objects
Encoder encX(encXA, encXB);
Encoder encY(encYA, encYB);
Encoder encZ(encZA, encZB);
Encoder encX(encXB, encXA);
Encoder encY(encYB, encYA);
Encoder encZ(encZB, encZA);
byte enablepins[]={enableX,enableY,enableZ};
......@@ -46,6 +54,8 @@ byte limitMpins[]={limitXM,limitYM,limitZM};
byte limitPpins[]={limitXP,limitYP,limitZP};
byte readypins[]={readyX,readyY,readyZ};
byte refpins[]={refX,refY,refZ};
byte brakepins[]={brakeX,brakeY,brakeZ};
void reportinputs()
{
......@@ -229,7 +239,7 @@ speed[ax]=null_speed;
mot_off(ax);
pos[ax]=0;
#ifdef info
Serial.println("homing of ax "+str(ax)+" done");
//Serial.println("homing of ax "+str(ax)+" done");
#endif
}
......@@ -302,6 +312,13 @@ digitalWrite(enableX,LOW);
digitalWrite(enableY,LOW);
digitalWrite(enableZ,LOW);
pinMode(brakeX,OUTPUT);
pinMode(brakeY,OUTPUT);
pinMode(brakeZ,OUTPUT);
digitalWrite(brakeX,LOW);
digitalWrite(brakeY,LOW);
digitalWrite(brakeZ,LOW);
pinMode(limitXP,INPUT);
......@@ -317,7 +334,7 @@ pinMode(readyX,INPUT);
pinMode(readyY,INPUT);
pinMode(readyZ,INPUT);
/*
digitalWrite(limitXP,HIGH);
digitalWrite(limitXM,HIGH);
digitalWrite(limitYP,HIGH);
......@@ -330,7 +347,7 @@ digitalWrite(refZ,HIGH);
digitalWrite(readyX,HIGH);
digitalWrite(readyY,HIGH);
digitalWrite(readyZ,HIGH);
*/
pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
......@@ -339,7 +356,35 @@ Serial.begin(115200);
Serial.println("\fonline...");
for (int l=0;l<1000;l++) {reportinputs();delay(1000);}
setbrake(0,false);
setbrake(1,true);
setbrake(2,true);
mot_on(0);
for (int m=0;m<1000;m++) {
/*reportinputs();
delay(1000);
for (int ab=0;ab<3;ab++)
{setbrake(ab,true);delay(1000);setbrake(ab,false);delay(1000);}
*/
for (int ab=0;ab<3;ab++)
{
mot_on(ab);
digitalWrite(led,HIGH);
delay(300);
mot_off(ab);
digitalWrite(led,LOW);
delay(300);
}
}
/*
attachInterrupt(0, doEncoderX, RISING); // encoder pin on interrupt 0 - pin 2
......@@ -348,24 +393,30 @@ attachInterrupt(2, doEncoderZ, RISING); // encoder pin on interrupt 1 - pin 3
*/
for (int m=0;m<max_ax;m++) set_ltc();
//for (int m=0;m<max_ax;m++) set_ltc();
for (int a=0;a<max_ax;a++) for (int s=0;s<10;s++) targets[a][s]=maxpos[a]+1;
//for (int a=0;a<max_ax;a++) for (int s=0;s<10;s++) targets[a][s]=maxpos[a]+1;
set_ltc();
}
void setbrake(int ax, bool val)
{
digitalWrite(brakepins[ax],not val);
}
void mot_on(int id)
{
if (enablepins[id] !=0) digitalWrite(enablepins[id],HIGH);
digitalWrite(enablepins[id],true);
}
void mot_off(int id)
{
if (enablepins[id] !=0) digitalWrite(enablepins[id],LOW);
digitalWrite(enablepins[id],false);
}
......@@ -375,6 +426,13 @@ void getEncoders()
pos[1]=encY.read();
pos[2]=encZ.read();
Serial.print(pos[0]);
Serial.print("\t");
Serial.print(pos[1]);
Serial.print("\t");
Serial.println(pos[2]);
}
void setEncoder(int ax,long val)
......@@ -402,6 +460,8 @@ void loop()
}
getEncoders();
for (int a=0;a<max_ax;a++)
{
testpos(a);
......@@ -416,6 +476,10 @@ void loop()
if (running!=lastrunning)
{
if (running) Serial.println("running");
else
{
......@@ -495,9 +559,9 @@ while (Serial.available() > 0) {
isaxcmd=false;
if (cmd=="b") {speed[1]+=20;Serial.println(speed[1]-null_speed);set_ltc();}
if (cmd=="b") {speed[0]+=20;Serial.println(speed[0]-null_speed);set_ltc();}
else
if (cmd=="c") {speed[1]-=20;Serial.println(speed[1]-null_speed);set_ltc();}
if (cmd=="c") {speed[0]-=20;Serial.println(speed[0]-null_speed);set_ltc();}
else
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment