Commit 6607ec5d authored by 42loop's avatar 42loop
Browse files

some cleanup

parent ff470b55
......@@ -63,25 +63,6 @@ Serial.println();
}
bool waitready()
{
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 !");
}
}
return servoready;
}
void setup()
{
......
void initpins()
{
pinMode(ltc_clk,OUTPUT);
//ltc DA-converter
pinMode(ltc_clk,OUTPUT);
pinMode(ltc_data,OUTPUT);
pinMode(ltc_load,OUTPUT);
pinMode(mux_enable,OUTPUT);
......@@ -16,7 +16,7 @@ digitalWrite(mux_a2,LOW);
digitalWrite(ltc_clk,LOW);
digitalWrite(ltc_load,HIGH);
//inverter start plus direction
pinMode(startXP,OUTPUT);
pinMode(startYP,OUTPUT);
pinMode(startZP,OUTPUT);
......@@ -25,6 +25,7 @@ digitalWrite(startXP,LOW);
digitalWrite(startYP,LOW);
digitalWrite(startZP,LOW);
//inverter start minus direction
pinMode(startXM,OUTPUT);
pinMode(startYM,OUTPUT);
pinMode(startZM,OUTPUT);
......@@ -33,6 +34,7 @@ digitalWrite(startXM,LOW);
digitalWrite(startYM,LOW);
digitalWrite(startZM,LOW);
//brake outputs
pinMode(brakeX,OUTPUT);
pinMode(brakeY,OUTPUT);
pinMode(brakeZ,OUTPUT);
......@@ -41,7 +43,7 @@ digitalWrite(brakeX,LOW);
digitalWrite(brakeY,LOW);
digitalWrite(brakeZ,LOW);
//limit and ref inputs
pinMode(limitXPpin,INPUT);
pinMode(limitXMpin,INPUT);
pinMode(limitYPpin,INPUT);
......@@ -52,12 +54,6 @@ pinMode(refX,INPUT);
pinMode(refY,INPUT);
pinMode(refZ,INPUT);
/*
pinMode(readyXpin,INPUT);
pinMode(readyYpin,INPUT);
pinMode(readyZpin,INPUT);
*/
digitalWrite(limitXPpin,HIGH);
digitalWrite(limitXMpin,HIGH);
......@@ -69,13 +65,8 @@ digitalWrite(limitZMpin,HIGH);
digitalWrite(refX,HIGH);
digitalWrite(refY,HIGH);
digitalWrite(refZ,HIGH);
/*
digitalWrite(readyXpin,HIGH);
digitalWrite(readyYpin,HIGH);
digitalWrite(readyZpin,HIGH);
*/
pinMode(led,OUTPUT);
pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
}
......@@ -2,70 +2,66 @@
void set_motors()
{
byte currentmotor=0;
//Serial.print("setltc: ");Serial.println(speed[0]);
for (currentmotor=0;currentmotor<max_ax;currentmotor++)
{
unsigned int val;
//invert values:
if (speed[currentmotor]>null_speed) //plus
{
val=speed[currentmotor]-null_speed;
mot_onP(currentmotor);
setbrake(currentmotor,false);
}
else if (speed[currentmotor]<null_speed) //minus
byte currentmotor=0;
for (currentmotor=0;currentmotor<max_ax;currentmotor++)
{
val=abs(speed[currentmotor]-null_speed);
mot_onM(currentmotor);
setbrake(currentmotor,false);
}
else
{
val=0; //stop
mot_off(currentmotor);
setbrake(currentmotor,true);
}
Serial.println(currentmotor,val);
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);
unsigned int val;
//adapt values: former 0 was max minus speed, null_speed was stop, 4095 was max plus speed
if (speed[currentmotor]>null_speed) //plus
{
val=speed[currentmotor]-null_speed;
mot_onP(currentmotor);
setbrake(currentmotor,false);
}
else if (speed[currentmotor]<null_speed) //minus
{
val=abs(speed[currentmotor]-null_speed);
mot_onM(currentmotor);
setbrake(currentmotor,false);
}
else
{
val=0; //stop
mot_off(currentmotor);
setbrake(currentmotor,true);
}
Serial.println(currentmotor,val);
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);
}
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_clk,HIGH);
delayMicroseconds(1);
digitalWrite(ltc_clk,LOW);
delayMicroseconds(1);
}
digitalWrite(ltc_load,LOW);
delayMicroseconds(1);
digitalWrite(ltc_load,HIGH);
digitalWrite(ltc_load,HIGH);
delayMicroseconds(400);
delayMicroseconds(400);
digitalWrite(mux_enable,HIGH);
digitalWrite(mux_enable,HIGH);
//probably not needed:
//delayMicroseconds(400);
}
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment