no_rt_motor.c

//  How to access GPIO registers from C-code on the Raspberry-Pi
//  Example program
//  15-January-2012
//  Dom and Gert
//  Revised: 15-Feb-2013
 
// Access from ARM Running Linux
  	 
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <pthread.h>
#include <time.h>
#include <math.h>

#define BCM2708_PERI_BASE   0x3F000000                                                                                    
#define GPIO_BASE           (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
#define PAGE_SIZE           (4*1024)
#define BLOCK_SIZE          (4*1024)
	   
int mem_fd;
void *gpio_map;
int time_interval = 1000;

// I/O access
volatile unsigned *gpio;
	  
// GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or SET_GPIO_ALT(x,y)
#define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3)) 
#define OUT_GPIO(g) *(gpio+((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))
		   
#define GPIO_SET *(gpio+7)  // sets   bits which are 1 ignores bits which are 0
#define GPIO_CLR *(gpio+10) // clears bits which are 1 ignores bits which are 0
		    
#define GET_GPIO(g) (*(gpio+13)&(1<<g)) // 0 if LOW, (1<<g) if HIGH
			 
#define GPIO_PULL *(gpio+37) // Pull up/pull down
#define GPIO_PULLCLK0 *(gpio+38) // Pull up/pull down clock
			  
void setup_io();
void *listen_rate();

int main(int argc, char **argv) {
							   
    // Set up gpi pointer for direct register access
    setup_io();

    mlockall(MCL_CURRENT | MCL_FUTURE);

    INP_GPIO(20);
    INP_GPIO(21);
    INP_GPIO(5);
	OUT_GPIO(5);
    INP_GPIO(6);
	OUT_GPIO(6);

    int A_curr = 0;
	int B_curr = 0;
	int A_pre = 0;
	int B_pre = 0;
	int count = 0;

    if( GET_GPIO(20) ){
        if( GET_GPIO(21) ){
            A_pre = 1;                                                                                                    
            B_pre = 1;
        }else{
            A_pre = 1;
			B_pre = 0;
        }
    }else{
        if( GET_GPIO(21) ){
            A_pre = 0;
			B_pre = 1;
        }else{
            A_pre = 0;
			B_pre = 0;
        }
    }

    while(1){
        if( GET_GPIO(20) ){
		    if( GET_GPIO(21) ){
                A_curr = 1;
				B_curr = 1;
			}else{
                A_curr = 1;
				B_curr = 0;
			}
		}else{
            if( GET_GPIO(21) ){
                A_curr = 0;
				B_curr = 1;
            }else{
                A_curr = 0;
				B_curr = 0;
            }
		}

        if( A_curr != A_pre || B_curr != B_pre) {
            
			if( A_pre == 1 && B_pre == 0 ) {
                if( A_curr == 1 ){
                    GPIO_SET = 1 << 6;
				}else{
                    GPIO_CLR = 1 << 6;
				}
			}else if( A_pre == 1 && B_pre == 1 ){
                if( A_curr == 0 ){
                    GPIO_SET = 1 << 6;
                }else{
                    GPIO_CLR = 1 << 6;
                }
			}else if( A_pre == 0 && B_pre == 1 ){
                if( A_curr == 0 ){
                    GPIO_SET = 1 << 6;
                }else{                                                                                                         
                    GPIO_CLR = 1 << 6;
                }
            }else if( A_pre == 0 && B_pre == 0 ){
                if( A_curr == 1 ){
                    GPIO_SET = 1 << 6;
                }else{                                                                                                         
                    GPIO_CLR = 1 << 6;
                }
            }

			GPIO_SET = 1 << 5;usleep(1);
			GPIO_CLR = 1 << 5;
			
			A_pre = A_curr;
			B_pre = B_curr;
        }
    }
    return 0;
}

//
// Set up a memory regions to access GPIO
//

void setup_io() {

    /* open /dev/mem */
    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
        printf("can't open /dev/mem \n");
        exit(-1);
    }
    /* mmap GPIO */
    gpio_map = mmap( NULL,             //Any adddress in our space will do
			         BLOCK_SIZE,       //Map length
                     PROT_READ|PROT_WRITE,// Enable reading & writting to mapped memory
                     MAP_SHARED,       //Shared with other processes
                     mem_fd,           //File to map
                     GPIO_BASE         //Offset to GPIO peripheral
    );

    close(mem_fd); //No need to keep mem_fd open after mmap
 
    if (gpio_map == MAP_FAILED) {
        printf("mmap error %d\n", (int)gpio_map);//errno also set!
        exit(-1);
    }
 
    // Always use volatile pointer!
    gpio = (volatile unsigned *)gpio_map;
}