further adaption to real hardware

geku_teensy.ino

-/////#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