RaspberryPi drive motor sample

raspberrypi_drive_motor.c

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>

#define PERI_BASE 0x3F000000
#define GPIO_BASE (PERI_BASE + 0x200000)
#define PWM_BASE  (PERI_BASE + 0x20C000)
#define CLK_BASE  (PERI_BASE + 0x101000)

#define CLK_PASSWD 0x5A000000

#define PAGE_SIZE (4*1024)
#define BLOCK_SIZE (4*1024)

// I/O access
volatile unsigned *gpio;
volatile unsigned *pwm;
volatile unsigned *clk;

static   int      mem_fd = 0;

void check( int condition, char* msg )
{
  if( condition ){ printf("%s", msg); exit (-1);}
}

volatile unsigned * io_mapping(int base_addr)
{
    char *gpio_mem, *gpio_map;
    
    if (!mem_fd) {
      mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
      check((mem_fd < 0), "can't open /dev/mem \n");
    }
    
    gpio_mem = malloc(BLOCK_SIZE + (PAGE_SIZE-1));
    check( ( gpio_mem == NULL), "allocation error \n");

    // Make sure pointer is on 4K boundary
    if ((unsigned long) gpio_mem % PAGE_SIZE)
      gpio_mem += PAGE_SIZE - ((unsigned long)gpio_mem % PAGE_SIZE);
    
    // Now map it
    gpio_map = (char *)mmap((caddr_t)gpio_mem, 
                            BLOCK_SIZE, 
                            PROT_READ|PROT_WRITE, 
                            MAP_SHARED|MAP_FIXED, 
                            mem_fd, 
                            base_addr );

    check( ((long)gpio_map < 0), "mmap error \n");

    return (volatile unsigned *)gpio_map;
}

void main()
{
  volatile unsigned *addr;
  int count;

  //--------------------------
  // GPIO
  gpio = io_mapping(GPIO_BASE);

  // set GPIO18 mode to ALT5(PWM0)
  *(gpio + 0x4/4) = (*(gpio + 0x4/4) & ~(0x7 << 24 )) | (0b010 << 24);

  // set GPIO20/21 mode to Output
  *(gpio + 0x8/4) = (*(gpio + 0x8/4) & ~(0x7 << 0 )) | (0b001 << 0);
  *(gpio + 0x8/4) = (*(gpio + 0x8/4) & ~(0x7 << 3 )) | (0b001 << 3);

  //--------------------------
  // Clock
  clk  = io_mapping(CLK_BASE);
  // add the offset address of pwm clk manager
  clk += 0xA0/4;
  // disable
  *clk = CLK_PASSWD + (0x1<<5);
  // divided by 192
  *(clk+0x4/4) = CLK_PASSWD + (192 << 12);
  // choose oscillator(19.2MHz) as source
  *clk = CLK_PASSWD + 0x1 + (0x1 << 4);

  //--------------------------
  // PWM
  pwm  = io_mapping(PWM_BASE);
  *pwm = 0; // Disable
  usleep(10);

  *(pwm + 0x10/4) = 8; // set RNG
  *(pwm + 0x14/4) = 6; // set DAT
  *pwm = 0x1;  // Enable

  //--------------------------
  // drive motor
  for( count = 0; count < 2; count++ ){
    // set
    if( count % 2 ){
      *(gpio + 0x1C/4) = 0x1 << 20;
    } else {
      *(gpio + 0x1C/4) = 0x1 << 21;
    }
    sleep(3); 

    // clear
    *(gpio + 0x28/4) = 0x1 << 21;
    *(gpio + 0x28/4) = 0x1 << 20;
    sleep(3); 
  }

 *pwm = 0; // Disable
  usleep(10);
}