some cleanup

geku_teensy.ino

@@ -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()
 {

initpins.ino

 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);
 
 }

motor_out.ino

@@ -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);
 
-}
+  }
 
 }