Pàgines

Jul 21, 2011

Simple controlador d'un 'Stepper' amb Arduino

Amb els xips L293D i 7400 podem fer un cotrolador sumple d'un motor pas a pas.

Funciona bé, però només podem controlar el motor en 'full step', motiu per el qual a velocitats baixes es produeix 'torque ripple' cosa la qual calenta el motor, augmenta el soroll i en el meu cas, fa que algunes parts de la màquina entrin en resonància (cambiaré aquest controlador per un que suporti 'micro stepping').

l'esquema és el següent:



un sencill programa per Arduino que controla el(s) motor(s):

/*
http://www.tigoe.net/pcomp/code/category/arduinowiring/51
http://www.bricogeek.com/shop/upload/datasheets/arduino/Motor_control_v3.0_sch.pdf
http://arduino.cc/en/Tutorial/MotorKnob
*/

class StepperMotor{
  private:
  ;
  public:
    byte motorPin1;
    byte motorPin2;
    byte topePin1;          // Tope -
    byte topePin2;          // Tope +
    int motorSteps;
    int pasosPermm;
    int actualStep;
    int direccio;
    unsigned long stepsLeft;
    boolean running;
    
    StepperMotor(byte lmotorPin1, byte lmotorPin2, byte ltopePin1, byte ltopePin2, int lmotorSteps, int lpasosPermm){
      motorPin1 = lmotorPin1;
      motorPin2 = lmotorPin2;
      topePin1 = ltopePin1;        // Tope -
      topePin2 = ltopePin2;        // Tope +      
      motorSteps = lmotorSteps;  // Steps per volta. NO Usat
      pasosPermm = lpasosPermm;
      actualStep = 0;
      running = false;
      
      pinMode(motorPin1,OUTPUT);
      pinMode(motorPin2,OUTPUT);
      pinMode(topePin1,INPUT);
      pinMode(topePin2,INPUT);    
    }
    
    void run(float lmm, int ldireccio){
      direccio = ldireccio;
      stepsLeft = lmm * (float)pasosPermm;
      running = true;
    }
    
    void playStep(){
      if(stepsLeft > 0 && pucAvansar(direccio)){
        switch (actualStep) {
          case 0: digitalWrite(motorPin1, LOW);  digitalWrite(motorPin2, HIGH);  break;  // 01
          case 1: digitalWrite(motorPin2, HIGH); digitalWrite(motorPin1, HIGH);  break;  // 11
          case 2: digitalWrite(motorPin1, HIGH); digitalWrite(motorPin2, LOW);   break;  // 10
          case 3: digitalWrite(motorPin2, LOW);  digitalWrite(motorPin1, LOW);   break;  // 00
        }
        actualStep += direccio;
        if(actualStep < 0) actualStep = 3; else if(actualStep > 3) actualStep = 0;
        stepsLeft--;
        if(stepsLeft == 0) running = false;
      }
    }
    
    boolean pucAvansar(int direccio){  // Retorna false si no es pot avançar en la dirreció actual
      switch (direccio){
        case -1:
          if(digitalRead(topePin1)) {running =false; return(false);} else return(true);
        case 1:
          if(digitalRead(topePin2)) {running =false; return(false);} else return(true);
      }
    }
    
    void goHome(){
      direccio = -1;
      while(pucAvansar(direccio)){
        switch (actualStep) {
          case 0: digitalWrite(motorPin1, LOW);  digitalWrite(motorPin2, HIGH);  break;  // 01
          case 1: digitalWrite(motorPin1, HIGH); digitalWrite(motorPin2, HIGH);  break;  // 11
          case 2: digitalWrite(motorPin1, HIGH); digitalWrite(motorPin2, LOW);   break;  // 10
          case 3: digitalWrite(motorPin1, LOW);  digitalWrite(motorPin2, LOW);   break;  // 00
        }
        actualStep += direccio;
        if(actualStep < 0) actualStep = 3; else if(actualStep > 3) actualStep = 0;
        delayMicroseconds(1300);
      }
    }
};





#include <flexitimer2.h> //http://arduino.cc/playground/Main/FlexiTimer2

const int nombreMotors = 3;
StepperMotor motors[nombreMotors] = {StepperMotor( 6,  7, 14, 15, 200,201)
                                    ,StepperMotor( 8,  9, 16, 17, 200,201)
                                    ,StepperMotor(10, 11, 18, 19, 200,201)};

void setup() {
  Serial.begin(115200); // Initialize the Serial port:
  
 // allMotorsHome();
  FlexiTimer2::set(3, 1.0/2000, driveSteppers);  // equivalent a delayMicroseconds(1500);
  FlexiTimer2::start();
}


void loop() {
  char eix = '  ';
  int mm;

  mm = procesaComandaSerie(&eix);
  if(mm !=0){
    switch(eix){
      case 'x':  case 'X':
        motors[0].run(abs(mm), mm/abs(mm)); break;
      case 'y': case 'Y':
        motors[1].run(abs(mm), mm/abs(mm)); break;
      case 'z': case 'Z':
        motors[2].run(abs(mm), mm/abs(mm)); break;
    }
  }
}


void allMotorsHome(){
  
  for(int i = 0; i < nombreMotors; i++){
    motors[i].goHome();
  }
}


void driveSteppers(){
  for(int i = 0; i < nombreMotors; i++){
    motors[i].playStep();
  }
}


   


int procesaComandaSerie(char *eix){
  String comanda = "";
  int retorn = 0;
  
  comanda = getSerialCommand();
  if(comanda.length()>0){
    Serial.println("#"+comanda+"#");
    *eix = comanda.charAt(0);
    Serial.println(*eix);
    comanda = comanda.substring(1);
    retorn = strToInt(comanda);
  }
  return(retorn);
}

String getSerialCommand(){
  String retorn="";
  char llegit;
     
  if(Serial.available()){
    llegit = Serial.read();
    while (llegit != 32){      
      if(Serial.available()){
        retorn += llegit;
        llegit = Serial.read();        
      }
    }
  }
  return(retorn);
}


int strToInt(String sNum){
  int retorn = 0;
  int pes = 1;
  
  for(int n=sNum.length()-1; n>=0; n--){
    if(sNum.charAt(n) == '-'){
      retorn *= -1;
    }else{
      retorn += (byte(sNum.charAt(n))-48)*pes;
      pes *= 10;
    }
  }
  Serial.println(retorn);
  return(retorn);
}

aques petit programa acepta comandes des de la consola sèrie de la forma 'x100 y-20 z300 ' (els espais son imprescindibles) que fa avançar 100mm en X -20mm en Y i 300mm en Z. donada les caractrística de la màquina (en el meu cas 'pasosPermm = 201' pasos per mm).