Hackster will be offline on Monday, June 15 from 5pm to 7pm PDT to perform some scheduled maintenance.
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 hours2,764
Driving Motors with Encoders on the Kria KR260

Things used in this project

Hardware components

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

AMD Vivado Design Suite
AMD Kria™ KV260 Vision AI Starter Kit

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
178 projects • 2034 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
  • FPGAs
  • Robotics
  • Robotics