Commit 6b9507ee authored by 42loop's avatar 42loop
Browse files

Initial commit

parents
/////#define debug
/////#define info
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#define encXA 2
#define encXB 9
#define encYA 3
#define encYB 19
#define encZA 0
#define encZB 0
Encoder encX(encXA, encXB);
Encoder encY(encYA, encYB);
Encoder encZ(encZA, encZB);
#define limitXM 12
#define limitXP 12
#define refX 12
#define limitYM 12
#define limitYP 12
#define refY 12
#define limitZM 12
#define limitZP 12
#define refZ 12
#define enableX 8
#define enableY 11
#define enableZ 11
//adjust to actual pin numbers:
byte enablepins[]={enableX,enableY,enableZ};
byte limitMpins[]={limitXM,limitYM,limitZM};
byte limitPpins[]={limitXP,limitYP,limitZP};
byte refpins[]={refX,refY,refZ};
//da-wandler:
#define ltc_load 4
#define ltc_clk 5
#define ltc_data 6
#define mux_enable 7
#define mux_a0 10
#define mux_a1 14
#define mux_a2 15
#define led 13
#define max_ax 3
//serial communication vars
int inByte;
String rec="";
int seq=0;
boolean repeat=false;
boolean running=false;
boolean lastrunning=false;
boolean issequence=false;
byte currentmotor=0;
const int hw_max_spd=4095;
const int hw_min_spd=0;
const int a10a8_max_spd=2300;
const int a10a8_min_spd=1800;
const int null_speed=2048;
//acceleration / braking states:
// 0: up
// 1: accel
// 2: max
// 3: braking
const byte r_stop=0;
const byte r_up=1;
const byte r_down=2;
const byte r_max=3;
const byte r_slow=4;
const byte d_stop=0;
const byte d_up=1;
const byte d_down=2;
byte m[]={5,5,5,5};
byte ramp[]={0,0,0,0};
byte dir[]={0,0,0,0};
int speed[]={null_speed,null_speed,null_speed,null_speed};
int absmaxupspeed[]={hw_max_spd,hw_max_spd,hw_max_spd,hw_max_spd};
int absmaxdownspeed[]={hw_min_spd,hw_min_spd,hw_min_spd,hw_min_spd};
int step[]={0,0,0,0};
int trackspeed[]={0,0,0,0};
int maxdownspeed[]={0,0,0,0};
int maxupspeed[]={0,0,0,0};
int minpos[]={0,0,0,0};
int maxpos[]={0,0,0,0};
int mindownspeed[]={null_speed-50,null_speed-50,null_speed-50};
int minupspeed[]={null_speed+50,null_speed+50,null_speed+50,};
boolean gopos[]={false,false,false,false};
boolean sak[]={true,true,true,true};
boolean enable[]={false,false,false,false};
volatile int pos[]={0,0,0,0};
volatile int target[]={0,0,0,0};
volatile int dist[]={0,0,0,0};
volatile int originpos[]={0,0,0,0};
int targets[4][10]={{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0}};
void showallspeeds()
{
#ifdef info
Serial.print("speeds: \t");
#endif
for (int s=0;s<max_ax;s++) {Serial.print(speed[s]);Serial.print('\t');}
Serial.println();
}
/*
//interrupt service routine
void doEncoderX()
{
if (digitalRead(encXA) == digitalRead(encXB)) {
pos[0]++;
} else {
pos[0]--;
}
//Serial.print('\f');
Serial.println(pos[0]);
}
void doEncoderY()
{
if (digitalRead(encYA) == digitalRead(encYB)) {
pos[1]--;
} else {
pos[1]++;
}
Serial.println(pos[1]);
}
void doEncoderZ()
{
if (digitalRead(encZA) == digitalRead(encZB)) {
pos[2]++;
} else {
pos[2]--;
}
//Serial.print('\f');
Serial.println(pos[2]);
}
*/
void home(int ax)
{
boolean islimit=false;
//move to minus endstop
mot_on(ax);
speed[ax]=null_speed-300;
//showallspeeds();
while (!islimit)
{
set_ltc();
delay(1);
if (!digitalRead(limitMpins[ax])) islimit=true;
}
speed[ax]=null_speed;
mot_off(ax);
delay(1000);
//move to plus endstop
mot_on(ax);
speed[ax]=null_speed+300;
islimit=false;
while (!islimit)
{
set_ltc();
delay(1);
if (!digitalRead(limitPpins[ax])) islimit=true;
}
speed[ax]=null_speed;
mot_off(ax);
pos[ax]=0;
#ifdef info
Serial.println("homing of ax "+str(ax)+" done");
#endif
}
void set_ltc()
{
currentmotor++;
if (currentmotor==max_ax) currentmotor=0;
unsigned int val;
val=speed[currentmotor];
digitalWrite(mux_enable,LOW);
if ((currentmotor & 1) !=0) digitalWrite(mux_a0,HIGH); else digitalWrite(mux_a0,LOW);
if ((currentmotor & 2) !=0) digitalWrite(mux_a1,HIGH); else digitalWrite(mux_a1,LOW);
digitalWrite(mux_a2,LOW);
for (int i=12; i>0; i--) {
if ((val & (1 << (i-1)))>0) {
digitalWrite(ltc_data,HIGH);
}
else {
digitalWrite(ltc_data,LOW);
}
delayMicroseconds(1);
digitalWrite(ltc_clk,HIGH);
delayMicroseconds(1);
digitalWrite(ltc_clk,LOW);
delayMicroseconds(1);
}
digitalWrite(ltc_load,LOW);
delayMicroseconds(1);
digitalWrite(ltc_load,HIGH);
delayMicroseconds(400);
digitalWrite(mux_enable,HIGH);
}
void setup()
{
pinMode(ltc_clk,OUTPUT);
pinMode(ltc_data,OUTPUT);
pinMode(ltc_load,OUTPUT);
pinMode(mux_enable,OUTPUT);
pinMode(mux_a0,OUTPUT);
pinMode(mux_a1,OUTPUT);
pinMode(mux_a2,OUTPUT);
digitalWrite(mux_enable,LOW);
digitalWrite(mux_a0,LOW);
digitalWrite(mux_a1,LOW);
digitalWrite(mux_a2,LOW);
digitalWrite(ltc_clk,LOW);
digitalWrite(ltc_load,HIGH);
pinMode(enableX,OUTPUT);
pinMode(enableY,OUTPUT);
pinMode(enableZ,OUTPUT);
digitalWrite(enableX,LOW);
digitalWrite(enableY,LOW);
digitalWrite(enableZ,LOW);
pinMode(limitXP,INPUT);
pinMode(limitXM,INPUT);
pinMode(limitYP,INPUT);
pinMode(limitYM,INPUT);
pinMode(limitZP,INPUT);
pinMode(limitZM,INPUT);
pinMode(refX,INPUT);
pinMode(refY,INPUT);
pinMode(refZ,INPUT);
digitalWrite(limitXP,HIGH);
digitalWrite(limitXM,HIGH);
digitalWrite(limitYP,HIGH);
digitalWrite(limitYM,HIGH);
digitalWrite(limitZP,HIGH);
digitalWrite(limitZM,HIGH);
digitalWrite(refX,HIGH);
digitalWrite(refY,HIGH);
digitalWrite(refZ,HIGH);
pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
Serial.begin(115200);
Serial.println("\fonline...");
/*
attachInterrupt(0, doEncoderX, RISING); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, doEncoderY, RISING); // encoder pin on interrupt 1 - pin 3
attachInterrupt(2, doEncoderZ, RISING); // encoder pin on interrupt 1 - pin 3
*/
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;
set_ltc();
}
void mot_on(int id)
{
if (enablepins[id] !=0) digitalWrite(enablepins[id],HIGH);
}
void mot_off(int id)
{
if (enablepins[id] !=0) digitalWrite(enablepins[id],LOW);
}
void getEncoders()
{
pos[0]=encX.read();
pos[1]=encY.read();
pos[2]=encZ.read();
}
void setEncoder(int ax,long val)
{
switch (ax) {
case 0:encX.write(val);
break;
case 1:encY.write(val);
break;
case 2:encZ.write(val);
break;
}
}
void loop()
{
long t=micros();
for (int a=0;a<max_ax;a++)
{
if (speed[a]==null_speed) dir[a]=d_stop;
if (speed[a]>null_speed) dir[a]=d_up;
if (speed[a]<null_speed) dir[a]=d_down;
}
getEncoders();
for (int a=0;a<max_ax;a++)
{
testpos(a);
}
set_ltc();
running=false;
for (int a=0;a<max_ax;a++)
{
if (!sak[a]) running=true;
}
if (running!=lastrunning)
{
if (running) Serial.println("running");
else
{
Serial.println("stop");
if (issequence)
{
seq++;
if (targets[0][seq]!=maxpos[0]+1)
{
delay(1000);
target[0]=targets[0][seq];
goposax(0);
}
else
{
if (repeat)
{
seq=0;
if (targets[0][seq]!=maxpos[0]+1)
{
delay(1000);
target[0]=targets[0][seq];
goposax(0);
}
}
else issequence=false;
}
}
}
lastrunning=running;
}
check_serial();
//Serial.println(micros()-t);
//delay(1000);
}
/*(***************************************************************)*/
void check_serial()
{
while (Serial.available() > 0) {
// get incoming byte:
inByte = Serial.read();
boolean isaxcmd=false;
if (inByte!=10) {rec+=(char)inByte;}
else
{
// Serial.println("got:'"+rec+"'");
rec.trim();
int p=rec.indexOf(' ');
String cmd=rec.substring(0,p);
String param=rec.substring(rec.indexOf(' ')+1);
char aparam[param.length() + 1];
param.toCharArray(aparam, sizeof(aparam));
int iparam = atoi(aparam);
#ifdef debug
Serial.print("cmd: ");Serial.println(cmd);
Serial.print("param: ");Serial.println(param);
#endif
//Serial.println(iparam);
//return:
if (cmd.length()==0)
{
#ifdef info
Serial.println("emergency off");
#endif
mot_off(0);
}
isaxcmd=false;
if (cmd=="b") {speed[1]+=20;Serial.println(speed[1]-null_speed);set_ltc();}
else
if (cmd=="c") {speed[1]-=20;Serial.println(speed[1]-null_speed);set_ltc();}
else
if ((cmd=="s") || (cmd=="r"))
{
// Serial.println("got seq or rep ");
if (issequence || repeat)
{
if (issequence && (cmd=="s")) issequence=false;
if (repeat && (cmd=="r")) repeat=false;
}
else
{
if (cmd=="s")
{
#ifdef debug
Serial.print("seq on ax ");
#endif
}
else
{
#ifdef debug
Serial.print("repeat seq on ax ");
#endif
repeat=true;
}
char *p = aparam;
char *str;
str = strtok_r(p, " ", &p);
int sax=atoi(str);
Serial.println(sax);
for (int a=0;a<10;a++) targets[sax][a]=maxpos[sax]+1;
boolean valid=true;
int count=0;
while ((str = strtok_r(p, " ", &p)) != NULL) // delimiter is the space
{
int val=atoi(str);
if ((val>=minpos[sax]) && (val<=maxpos[sax])) targets[sax][count]=atoi(str);
else
{
#ifdef info
Serial.print("invalid: ");
#endif
Serial.println(val);
valid=false;
}
count++;
}
if (!valid)
{
for (int a=0;a<10;a++) targets[sax][a]=maxpos[sax]+1;
#ifdef info
Serial.println("invalid sequence, deleted");
#endif
}
count=0;
while (targets[sax][count]!=maxpos[sax]+1) {Serial.println(targets[sax][count]);count++;}
issequence=true;
seq=0;
target[sax]=targets[sax][seq];
goposax(sax);
}
}
if (cmd=="=") {home(0);}
if (cmd=="0")
{
#ifdef info
Serial.print("move ax ");
#endif
Serial.print(cmd);
Serial.print(" to ");
Serial.println(iparam);
if ((iparam<=maxpos[0]) && (iparam>=minpos[0]))
{
target[0]=iparam;
goposax(0);
}
else
{
#ifdef info
Serial.println("invalid position");
#endif
}
}
rec="";
}
}
}
/*(***************************************************************)*/
//set start values for certain ax
void goposax(int ax)
{
#ifdef info
Serial.print("calculating vals for ax ");
#endif
Serial.println(ax);
#ifdef info
Serial.print("target: ");
#endif
Serial.println(target[ax]);
#ifdef info
Serial.print("position: ");
#endif
Serial.println(pos[ax]);
step[ax]=0;
originpos[ax]=pos[ax];
maxdownspeed[ax]=0;
maxupspeed[ax]=0;
if (pos[ax]>target[ax])
{
dist[ax]=pos[ax]-target[ax];
trackspeed[ax]=2048-round((dist[ax]*6));
if (trackspeed[ax]>2018) trackspeed[ax]=2018;
maxdownspeed[ax]=trackspeed[ax];
if (maxdownspeed[ax]<absmaxdownspeed[ax]) maxdownspeed[ax]=absmaxdownspeed[ax];
speed[ax]=mindownspeed[ax];
ramp[ax]=r_up;
#ifdef info
Serial.println("accel");
#endif
}
else
{
dist[ax]=target[ax]-pos[ax];
trackspeed[ax]=2048+round(dist[ax]*6);
if (trackspeed[ax]<2078) trackspeed[ax]=2078;
maxupspeed[ax]=trackspeed[ax];
if (maxupspeed[ax]>absmaxupspeed[ax]) maxupspeed[ax]=absmaxupspeed[ax];
speed[ax]=minupspeed[ax];
ramp[ax]=r_up;
#ifdef info
Serial.println("accel");
#endif
}