Commit 9673678c authored by 42loop's avatar 42loop
Browse files

untested rework

parent 5b2a5f0a
//update analog out for the next ax
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);
}
#define debug
#define info
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
//encoder inputs, 5V signal
#define encXA 24
#define encXB 25
#define encYA 26
#define encYB 27
#define max_ax 2
#define encZA 28
#define encZB 29
//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 ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#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
#include "pins.h"
#include "globals.h"
//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
byte enablepins[]={enableX,enableY,enableZ};
byte brakepins[]={brakeX,brakeY,brakeZ};
//encoder objects
......@@ -48,48 +22,30 @@ Encoder encX(encXB, encXA);
Encoder encY(encYB, encYA);
Encoder encZ(encZB, encZA);
byte enablepins[]={enableX,enableY,enableZ};
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};
//hardware query timer
IntervalTimer hwTimer;
void reportinputs()
{
Serial.println("limitXM\tlimitYM\tlimitZM\tlimitXP\tlimitYP\tlimitZP\trefX\trefY\trefZ\treadyX\treadyY\treadyZ");
Serial.print(digitalRead(limitXM));Serial.print("\t");
Serial.print(digitalRead(limitYM));Serial.print("\t");
Serial.print(digitalRead(limitZM));Serial.print("\t");
Serial.print(digitalRead(limitXP));Serial.print("\t");
Serial.print(digitalRead(limitYP));Serial.print("\t");
Serial.print(digitalRead(limitZP));Serial.print("\t");
Serial.print(digitalRead(refX));Serial.print("\t");
Serial.print(digitalRead(refY));Serial.print("\t");
Serial.print(digitalRead(refZ));Serial.print("\t");
Serial.print(digitalRead(readyX));Serial.print("\t");
Serial.print(digitalRead(readyY));Serial.print("\t");
Serial.println(digitalRead(readyZ));
Serial.println("limitXM\tlimitYM\tlimitZM\tlimitXP\tlimitYP\tlimitZP\treadyX\treadyY\treadyZ");
Serial.print(limitXM);Serial.print("\t");
Serial.print(limitYM);Serial.print("\t");
Serial.print(limitZM);Serial.print("\t");
Serial.print(limitXP);Serial.print("\t");
Serial.print(limitYP);Serial.print("\t");
Serial.print(limitZP);Serial.print("\t");
Serial.print(readyX);Serial.print("\t");
Serial.print(readyY);Serial.print("\t");
Serial.println(readyZ);
}
//da-wandler:
#define ltc_load 36
#define ltc_clk 37
#define ltc_data 38
#define mux_enable 39
#define mux_a0 30
#define mux_a1 31
#define mux_a2 32
#define led 13
#define max_ax 3
//serial communication vars
int inByte;
......@@ -102,7 +58,7 @@ boolean running=false;
boolean lastrunning=false;
boolean issequence=false;
byte currentmotor=0;
volatile byte currentmotor=0;
const int hw_max_spd=4095;
const int hw_min_spd=0;
......@@ -128,6 +84,7 @@ const byte d_up=1;
const byte d_down=2;
//acceleration
byte m[]={5,5,5,5};
byte ramp[]={0,0,0,0};
......@@ -170,209 +127,79 @@ 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()
bool waitready()
{
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);
bool servoready=true;
for (int ax=0;ax<max_ax;ax++)
{
if (!machineready[ax])
{
servoready=false;
Serial.print("Servo ");
Serial.print(ax);
Serial.println(" not ready !");
}
delayMicroseconds(1);
digitalWrite(ltc_clk,HIGH);
delayMicroseconds(1);
digitalWrite(ltc_clk,LOW);
delayMicroseconds(1);
}
digitalWrite(ltc_load,LOW);
delayMicroseconds(1);
digitalWrite(ltc_load,HIGH);
}
return servoready;
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(brakeX,OUTPUT);
pinMode(brakeY,OUTPUT);
pinMode(brakeZ,OUTPUT);
digitalWrite(brakeX,LOW);
digitalWrite(brakeY,LOW);
digitalWrite(brakeZ,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);
pinMode(readyX,INPUT);
pinMode(readyY,INPUT);
pinMode(readyZ,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);
digitalWrite(readyX,HIGH);
digitalWrite(readyY,HIGH);
digitalWrite(readyZ,HIGH);
pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
initpins();
Serial.begin(115200);
Serial.println("\fonline...");
setbrake(0,false);
setbrake(0,true);
setbrake(1,true);
setbrake(2,true);
hwTimer.begin(heartbeat, 150000); // blinkLED to run every 0.15 seconds
set_ltc();
mot_on(0);
while (!waitready())
{;}
for (int m=0;m<1000;m++) {
/*
delay(10000);
Serial.println("mot on");
mot_on(0);
delay(5000);
mot_off(0);
delay(10000);
Serial.println("mot on");
mot_on(1);
delay(5000);
mot_off(1);
*/
/*
for (int m=0;m<1000;m++) {
/*reportinputs();
reportinputs();
delay(1000);
}
*/
/*
for (int ab=0;ab<3;ab++)
{setbrake(ab,true);delay(1000);setbrake(ab,false);delay(1000);}
*/
/*
for (int m=0;m<1000;m++) {
for (int ab=0;ab<3;ab++)
{
mot_on(ab);
......@@ -383,22 +210,22 @@ digitalWrite(led,LOW);
delay(300);
}
}
*/
//for (int a=0;a<max_ax;a++) for (int s=0;s<10;s++) targets[a][s]=maxpos[a]+1;
/*
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();
/////////////////////////////////////////////////////////////////
/// end of setup
//for (int a=0;a<max_ax;a++) for (int s=0;s<10;s++) targets[a][s]=maxpos[a]+1;
set_ltc();
void heartbeat()
{
Serial.println("heartbeat");
getinputs();
}
......@@ -451,486 +278,63 @@ void setEncoder(int ax,long val)
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();
check_serial();
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();
planner();
//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[0]+=20;Serial.println(speed[0]-null_speed);set_ltc();}
else
if (cmd=="c") {speed[0]-=20;Serial.println(speed[0]-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