Initial commit

geku_teensy.ino

+/////#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
+    }
+#ifdef info 
+    Serial.print("distance: ");
+#endif
+Serial.println(dist[ax]);
+#ifdef info 
+    Serial.print("maxdownspeed: ");
+#endif
+Serial.println(maxdownspeed[ax]);
+#ifdef info 
+    Serial.print("maxupspeed: ");
+#endif
+Serial.println(maxupspeed[ax]);
+#ifdef info 
+    Serial.print("initial speed: ");
+#endif
+Serial.println(speed[ax]);
+
+
+
+    if (dist[ax]<2) //too small distance, ignore
+   {
+         gopos[ax]=false;
+         speed[ax]=null_speed;
+         ramp[ax]=r_stop;
+         sak[ax]=true;
+   }
+   else
+   {
+         mot_on(ax);
+         sak[ax]=false;
+         running=true;
+         gopos[ax]=true;
+   }
+
+}
+
+
+
+
+//testing ax positions for needed action:
+
+void testpos(int ax)
+{
+  int rampunit=1;
+  
+   if (gopos[ax]&&(dir[ax]==d_up))
+   {
+       if ((pos[ax]>originpos[ax]+step[ax]*rampunit)
+                 && (speed[ax]<maxupspeed[ax])
+                 && (ramp[ax]==r_up))
+                 {
+                  //(*BESCHLEUNIGEN*)
+                 
+                       speed[ax]=speed[ax]+m[ax];
+                       step[ax]++;
+   //                    Serial.print("speed: ");
+   //                    Serial.println(speed[ax]);
+                       if (speed[ax]>=maxupspeed[ax])
+                          {
+                                  ramp[ax]=r_max;
+                                  Serial.println("max");
+                                  speed[ax]=maxupspeed[ax];
+                          }
+                 }
+
+
+        if ((pos[ax]>target[ax]-step[ax]*rampunit)
+                  && (ramp[ax]==r_max))
+                  {
+                       ramp[ax]=r_down;
+           #ifdef info 
+            Serial.println("down");
+#endif
+step[ax]--;
+                  }
+
+
+        if (ramp[ax]==r_down)
+                  {
+                       if ((pos[ax]>target[ax]-step[ax]*rampunit)
+                       && (speed[ax]>minupspeed[ax]))
+
+                       {
+                          //(*BREMSEN*)
+                   
+                       if (step[ax]>0) step[ax]--;
+
+                       speed[ax]=speed[ax]-m[ax]*1;
+
+                       if (speed[ax]<=minupspeed[ax])
+                          {
+                              speed[ax]=minupspeed[ax];
+                              ramp[ax]=r_slow;
+#ifdef info 
+                              Serial.println("slow");
+#endif
+}
+
+                       }
+                  }
+
+        if ((ramp[ax]==r_slow)
+            &&(pos[ax]>=target[ax]))
+                  {
+                  //   (*STOP*)
+
+                   speed[ax]=2048;
+                   gopos[ax]=false;
+
+                    mot_off(ax);
+                    
+                    ramp[ax]=r_stop;
+#ifdef info 
+                    Serial.println("stop");
+#endif
+sak[ax]=true;
+
+                  }
+
+
+
+    }
+
+
+
+
+    if (gopos[ax] && (dir[ax]==d_down))
+         {
+            if ((ramp[ax]==r_up)
+                && (pos[ax]<(originpos[ax]-step[ax]*rampunit))
+                && (speed[ax]>maxdownspeed[ax]))
+
+                {//      (*BESCHLEUNIGEN*)
+                       speed[ax]=speed[ax]-m[ax];
+                       step[ax]++;
+//                       Serial.print("speed: ");
+//                       Serial.println(speed[ax]);
+
+                       if (speed[ax]<=maxdownspeed[ax])
+                        {
+                                   speed[ax]=maxdownspeed[ax];
+                                   ramp[ax]=r_max;
+#ifdef info 
+                                   Serial.println("max");
+#endif
+}
+                }
+
+
+               if ((ramp[ax]==r_max)
+                  && (pos[ax]<target[ax]+step[ax]*rampunit))
+                  {
+                    ramp[ax]=r_down;
+#ifdef info 
+                    Serial.println("down");
+#endif
+step[ax]--;
+                  }
+
+               if (ramp[ax]==r_down)
+               {
+                  if ((pos[ax]<target[ax]+step[ax]*rampunit)
+                  && (speed[ax]<mindownspeed[ax]))
+
+                  {//                            (*BREMSEN*)
+                       if (step[ax]>0) step[ax]--;
+                       speed[ax]=speed[ax]+m[ax]*1;
+                       if (speed[ax]>=mindownspeed[ax])
+                        {
+                           speed[ax]=mindownspeed[ax];
+                           ramp[ax]=r_slow;
+#ifdef info 
+                           Serial.println("slow");
+#endif
+}
+
+                  }
+
+               }
+
+              if ((ramp[ax]==r_slow)
+               && (pos[ax]<=target[ax]))
+               
+                  {//        (*STOP*)
+
+                   mot_off(ax);
+                   
+                   speed[ax]=2048;
+                   gopos[ax]=false;
+
+                    
+                   ramp[ax]=r_stop;
+#ifdef info 
+                   Serial.println("stop");
+#endif
+sak[ax]=true;
+                  }
+
+
+
+
+         }
+
+
+}
+
+/*(***************************************************************)*/
+
+