Whitney Knitter
Published © GPL3+

Driving Motors with Encoders on the Kria KR260

This project demonstrates how to drive an N20 micro-gear motor with encoder using a Digilent HB3 motor controller PMOD with the Kria KR260.

IntermediateFull instructions provided2 hours1,654
Driving Motors with Encoders on the Kria KR260

Things used in this project

Hardware components

Kria™ KR260 Robotics Starter Kit
AMD Kria™ KR260 Robotics Starter Kit
×1
Pmod HB3
Digilent Pmod HB3
×1
Adafruit N20 DC Motor with Magnetic Encoder - 6V with 1:100 Gear Ratio
×1

Software apps and online services

Vivado Design Suite
AMD Vivado Design Suite

Story

Read more

Schematics

PMOD HB3 Schematic

Code

motor_controller.c

C/C++
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <fcntl.h>
#include <signal.h>
#include <pthread.h>
#include <linux/input.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdbool.h>

#define IN         0
#define OUT        1
#define LOW        0
#define HIGH       1
#define DIR_MAX    35
#define VAL_MAX    33
#define BUFFER_MAX 3

#define PMOD1_BASE 492
#define PMOD2_BASE 484
#define PMOD3_BASE 476
#define PMOD4_BASE 468

#define REVERSE    0
#define FORWARD    1
#define MOTOR1_DIR 492
#define MOTOR1_EN  493
#define MOTOR1_SA  494
#define MOTOR1_SB  495

int export_pins(int pmod_base){
    char pmod_io[BUFFER_MAX];

    int fd = open("/sys/class/gpio/export", O_WRONLY);
    if (fd == -1){
        perror("Unable to open /sys/class/gpio/export");
        return(-1);
    }

    for (int i=pmod_base;i<(pmod_base+8);i++){
        sprintf(pmod_io, "%d",i);

        if (write(fd, pmod_io, 3) != 3){
            perror("Error writing to /sys/class/gpio/export");
            return(-1);
        }
    }

    close(fd);
    return(0);
}

int unexport_pins(int pmod_base){
    char pmod_io[BUFFER_MAX];

    int fd = open("/sys/class/gpio/unexport", O_WRONLY);
    if (fd == -1){
        perror("Unable to open /sys/class/gpio/unexport");
        return(-1);
    }

    for (int i=pmod_base;i<(pmod_base+8);i++){
        sprintf(pmod_io, "%d",i);

        if (write(fd, pmod_io, 3) != 3){
            perror("Error writing to /sys/class/gpio/unexport");
            return(-1);
        }
    }

    close(fd);
    return(0);
}

int gpio_direction(int pin, int dir){
    char dir_path[DIR_MAX];
    sprintf(dir_path, "/sys/class/gpio/gpio%d/direction", pin);

    int fd = open(dir_path, O_WRONLY);
    if (fd == -1){
        perror("Unable to open /sys/class/gpio/gpiox/direction");
        return(-1);
    }

    if (dir == OUT){
        if (write(fd, "out", 3) != 3){
            perror("Error writing to /sys/class/gpio/gpiox/direction");
            return(-1);
        }
    } else {
        if (write(fd, "in", 2) != 2){
            perror("Error writing to /sys/class/gpio/gpiox/direction");
            return(-1);
        }
    }

    close(fd);
    return(0);
}

int gpio_read(int pin){
    char val_path[VAL_MAX];
    sprintf(val_path, "/sys/class/gpio/gpio%d/value", pin);

    int fd = open(val_path, O_RDONLY);
    if (fd == -1){
        perror("Unable to open /sys/class/gpio/gpiox/value");
        return(-1);
    }

    char rd_buf[BUFFER_MAX];
    if (read(fd, rd_buf, 2) != 2){
        perror("Error reading from /sys/class/gpio/gpiox/value");
        //return(-1);
    }

    close(fd);
    return(atoi(rd_buf));
}

int gpio_write(int pin, int val){
    char val_path[VAL_MAX];
    sprintf(val_path, "/sys/class/gpio/gpio%d/value", pin);

    int fd = open(val_path, O_WRONLY);
    if (fd == -1){
        perror("Unable to open /sys/class/gpio/gpiox/value");
        return(-1);
    }

    if (val == HIGH){
        if (write(fd, "1", 1) != 1){
            perror("Error writing HIGH to /sys/class/gpio/gpiox/value");
            return(-1);
        }
    } else {
        if (write(fd, "0", 1) != 1){
            perror("Error writing LOW to /sys/class/gpio/gpiox/value");
            return(-1);
        }
    }

    close(fd);
    return(0);
}

int main(){
    int status = 0;
    int sensorA_val = 0;
    int sensorB_val = 0;

    status = export_pins(PMOD1_BASE);
    if (status == -1){
        perror("Error exporting pins!");
        return(-1);
    }

    status = gpio_direction(MOTOR1_DIR, OUT);
    if (status == -1){
        perror("Error setting MOTOR1_DIR!");
        return(-1);
    }

    status = gpio_direction(MOTOR1_EN, OUT);
    if (status == -1){
        perror("Error setting MOTOR1_EN!");
        return(-1);
    }

    status = gpio_direction(MOTOR1_SA, IN);
    if (status == -1){
        perror("Error setting MOTOR1_SA!");
        return(-1);
    }

    status = gpio_direction(MOTOR1_SB, IN);
    if (status == -1){
        perror("Error setting MOTOR1_SB!");
        return(-1);
    }

    // Direction pin must not change state while the
    // Enable pin is at a high voltage state!!!
    gpio_write(MOTOR1_DIR, FORWARD);
    gpio_write(MOTOR1_EN, HIGH);

    int sensorA_last = 0;
    int sensorB_last = 0;
    clock_t start_t, stop_t;
    double total_time;
    double rpm;
    bool T_start = false;

    for (int i=0;i<10000;i++){
        sensorA_val = gpio_read(MOTOR1_SA);
        sensorB_val = gpio_read(MOTOR1_SB);

        if ((sensorA_val == 1) && (sensorB_val == 0) && (sensorA_last == 0)){
            printf("MOTOR1 direction = REVERSE\n");

            if (T_start == true){
                stop_t = clock();
                T_start = false;

                total_time = (double)(stop_t - start_t) / CLOCKS_PER_SEC;
                rpm = 1/total_time; // revoluions
                rpm *= 60; //convert from sec to min
                rpm /= 100; //account from gearing ratio
                rpm /= 14; //acount for ticks per rotation
            } else {
                start_t = clock();
                T_start = true;
            }

            printf("RPM = %f\n", rpm);
        }

        if ((sensorA_val == 0) && (sensorB_val == 1) && (sensorB_last == 0)){
            printf("MOTOR1 direction = FORWARD\n"); 

            if (T_start == true){
                stop_t = clock();
                T_start = false;

                total_time = (double)(stop_t - start_t) / CLOCKS_PER_SEC;
                rpm = 1/total_time; // revoluions
                rpm *= 60; //convert from sec to min
                rpm /= 100; //account from gearing ratio
                rpm /= 14; //acount for ticks per rotation
            } else {
                start_t = clock();
                T_start = true;
            }

            printf("RPM = %f\n", rpm);
        }

        sensorA_last = sensorA_val;
        sensorB_last = sensorB_val;
    }


    // Direction pin must not change state while the
    // Enable pin is at a high voltage state!!!
    gpio_write(MOTOR1_EN, LOW);
    gpio_write(MOTOR1_DIR, LOW);

    status = unexport_pins(PMOD1_BASE);
    if (status == -1){
        perror("Error unexporting pins!");
        return(-1);
    }

    return(0);
}

Credits

Whitney Knitter

Whitney Knitter

157 projects • 1591 followers
All thoughts/opinions are my own and do not reflect those of any company/entity I currently/previously associate with.

Comments

Related channels and tags
  • Adafruit
  • AMD
  • Digilent
  • FPGAs
  • Robotics