[gl-como] 3ddy

[Davide De Prisco]

Il primo programma per 3ddy, scritto da Fabrix alla fiera di Erba

Tanto per averne traccia


##
/*Code from http://www.schmalzhaus.com/EasyDriver/Examples/Ea... */
#define INPUT_BUFFER 30

// digitalInputToInterrupt per Arduino Mega 2560
int digitalInputToInterrupt(byte pin){
  switch(pin) {
    case  2: return 0; break;
    case  3: return 1; break;
    case 18: return 5; break;
    case 19: return 4; break;
    case 20: return 3; break;
    case 21: return 2; break;
  }
}


#define RITARDO 500
class Motore {
  public:
    Motore(byte steppin, byte dirpin, byte fc1pin, byte fc2pin) {
      m_step = steppin;
      m_dir = dirpin;
      m_finecorsa1 = fc1pin;
      m_finecorsa2 = fc2pin;
      m_finecorsa1_triggered = false;
      m_finecorsa2_triggered = false;

      pinMode(dirpin, OUTPUT);
      pinMode(steppin, OUTPUT);
      digitalWrite(dirpin, LOW);
      digitalWrite(steppin, LOW);

      pinMode(fc1pin, INPUT_PULLUP);
      pinMode(fc2pin, INPUT_PULLUP);
    }

    bool step() {
      Serial.print("step() "); Serial.print(m_finecorsa1_triggered);
Serial.print(" - ");  Serial.println(m_finecorsa2_triggered);
      if (m_dir_corrente==0 && m_finecorsa1_triggered) {
Serial.println("stop by finecorsa1"); return true; }
      if (m_dir_corrente==1 && m_finecorsa2_triggered) {
Serial.println("stop by finecorsa2"); return true; }
      if (m_dir_corrente==1 && m_finecorsa1_triggered) {
        if (digitalRead(m_finecorsa1)) m_finecorsa1_triggered = false;
      }
      if (m_dir_corrente==0 && m_finecorsa2_triggered) {
        if (digitalRead(m_finecorsa2)) m_finecorsa2_triggered = false;
      }

      digitalWrite(m_step, HIGH);
      delayMicroseconds(RITARDO);
      digitalWrite(m_step, LOW);
      delayMicroseconds(RITARDO);
      return false;
    }

    void set_dir(byte thedir) {
      m_dir_corrente = thedir;
      digitalWrite(m_dir, thedir);
    }

    bool m_finecorsa1_triggered;
    bool m_finecorsa2_triggered;

    byte m_step;
    byte m_dir;
    byte m_finecorsa1;
    byte m_finecorsa2;
    byte m_dir_corrente;


};

int Distance = 0; // Record the number of steps we've taken
int dir = 0;
Motore mx = Motore(9, 8, 19 /* X  des */, 18 /* X sin */);
Motore my = Motore(11, 10, 21 /* Y sin */, 20 /* Y des */);

void int_motore_x_stop1() { mx.m_finecorsa1_triggered = true; }
void int_motore_x_stop2() { mx.m_finecorsa2_triggered = true; }
void int_motore_y_stop1() { my.m_finecorsa1_triggered = true; }
void int_motore_y_stop2() { my.m_finecorsa2_triggered = true; }

void setup() {
  Serial.begin(9600);

  attachInterrupt(digitalInputToInterrupt(mx.m_finecorsa1),
int_motore_x_stop1, FALLING);
  attachInterrupt(digitalInputToInterrupt(mx.m_finecorsa2),
int_motore_x_stop2, FALLING);
  attachInterrupt(digitalInputToInterrupt(my.m_finecorsa1),
int_motore_y_stop1, FALLING);
  attachInterrupt(digitalInputToInterrupt(my.m_finecorsa2),
int_motore_y_stop2, FALLING);
}

void loop() {
  char input[INPUT_BUFFER + 1];
  byte size = Serial.readBytes(input, INPUT_BUFFER);
  input[size] = 0;
  if (size>0) do_command(input);
}

void do_command(char *input) {
  //x 1 100
  //y 0 10

  if (input[0]=='r') {
    mx.m_finecorsa1_triggered = false;
    mx.m_finecorsa2_triggered = false;
    my.m_finecorsa1_triggered = false;
    my.m_finecorsa2_triggered = false;
    return;
  }

  // zero dove potrebbe servire
  input[1] = 0;
  input[3] = 0;

  char asse = input[0];
  int dir = atoi( &input[2] );
  char *rest = &input[4];
  int steps = atoi(rest);

  Serial.print("asse "); Serial.println(asse);
  Motore *motore;
  if (asse == 'x') motore = &mx;
  if (asse == 'y') motore = &my;

  Serial.print("dir "); Serial.println(dir);
  motore->set_dir(dir);

  for(int k=0; k<steps; k++) {
    if (motore->step()) break;
    //Serial.print("step "); Serial.println(k);
  }
}

##

--

Davide