Schrittmotor ansteuern

Motor- & Incrementalgeber-Test auf dem Zero W

Nachdem ich wusste das der Incrementalgeber funktionierte,
habe ich angefangen den Motor anzusteuern.
Zunächst soll er sich so lange drehen, bis die Null-Position erreicht ist.
Dann rotiere ich weiter und notiere ich mir nach jeden Motor-Schritt
die gemessene Position. Dann wiederhole ich die Rotation in andere Richtung.

nano mottst.c
#include <stdio.h>
#include <bcm2835.h>

#define nRes    RPI_V2_GPIO_P1_32             // dieses Mal die Pins und nicht die Ports
#define nStep   PRI_V2_GPIO_P1_26
#define dir     RPI_V2_GPIO_P1_24
#define nFault  RPI_V2_GPIO_P1_22
#define inA     RPI_V2_GPIO_P1_16
#define inB     RPI_V2_GPIO_P1_08
#define inN     RPI_V2_GPIO_P1_10

int main( void )
{ if( !bcm2835_init() ) { printf("no lib found\n"); return 1; }
  
  printf("motor test started\n");
  uint8_t inanew, inbnew, inaold, inbold;
  uint9_t innnew, innold, state;
  int tmpcnt, motcnt, inccnt, x;
  int tab[ 5000 ] = { 0 };

  bcm2835_gpio_fsel(  nRes, BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_write( nRes, LOW  );                   // reset DRV8825
  bcm2835_gpio_fsel(  dir,  BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_write( dir,  LOW  );
  bcm2835_gpio_fsel(  step, BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_write( step, LOW  );

  bcm2835_gpio_fsel( inA,    BCM2835_GPIO_FSEL_INPT);
  bcm2835_gpio_fsel( inB,    BCM2835_GPIO_FSEL_INPT);
  bcm2835_gpio_fsel( inN,    BCM2835_GPIO_FSEL_INPT);
  bcm2835_gpio_fsel( nFault, BCM2835_GPIO_FSEL_INPT);
  bcm2835_gpio_set_pud(nFault, BCM2835_GPIO_PUD_UP);  // pull-up for nFault

  bcm2835_gpio_write( nRes, HIGH );
  state = 0;                                          // turn motor to zero position
  tmpcnt = 0;
  motcnt = 0;
  inccnt = 0;
  inanew = bcm2835_gpio_lev(inA);                     // bcm2835_gpio_lev returns uint8_t
  inbnew = bcm2835_gpio_lev(inB);
  innnew = bcm2835_gpio_lev(inN);
  
  while( state < 2 )
  { tmpcnt++;
    
    if( tmpcnt > 7 )                                  // after 800 µs one step 
    { printf( " %d %d %d \n", tmpcnt, motcnt, inccnt );
      bcm2835_gpio_write( step, HIGH );
      delayMicroseconds( 50 );
      bcm2835_gpio_write( step, LOW );
      delayMicroseconds( 50 );
      tmpcnt = 0;
      motcnt++;
    }
    else
    { delayMicroseconds( 100 );                       // to pause program -> cpu x%
    }

    inaold = inanew;
    inbold = inbnew;
    innold = innnew;
    inanew = bcm2835_gpio_lev(inA);
    inbnew = bcm2835_gpio_lev(inB);
    innnew = bcm2835_gpio_lev(inN);
    
    if( innnew > innold ) { inccnt = 0; motcnt = 0; state++; }

    if( state > 0 )
    { if(      inanew > inaold && inbnew == 0 && inbold == 0 ) { inccnt++; }
      else if( inanew > inaold && inbnew == 1 && inbold == 1 ) { inccnt--; }
      else if( inanew < inaold && inbnew == 0 && inbold == 0 ) { inccnt--; }
      else if( inanew < inaold && inbnew == 1 && inbold == 1 ) { inccnt++; }
      else if( inanew == 0 && inaold == 0 && inbnew < inbold ) { inccnt++; }
      else if( inanew == 0 && inaold == 0 && inbnew > inbold ) { inccnt--; }
      else if( inanew == 1 && inaold == 1 && inbnew < inbold ) { inccnt--; }
      else if( inanew == 1 && inaold == 1 && inbnew > inbold ) { inccnt++; }
      
      else if( inanew != inaold && inbnew != inbold )
      { printf( "lost step\n" ); 
        bcm2835_gpio_write( nRes, LOW );              // disable motor
        return 2;
      }
      
      tab[ inccnt ] = motcnt;
    } // if( state > 0 )
  } // while( state < 2 )

  for( x = 0; x < 4096; x++ )
  { printf( "[%d] [%d] \n", x, tab[x] );
  }
}

Zum Anfang