IDCT Bartosz Pachołek

Cookies Warning

This website uses cookies: We inform you that this site uses own, technical and third parties cookies to make sure our web page is user-friendly and to guarantee a high functionality of the webpage and for statistics tracking. In order to use some (contact form) parts of the website you need to accept the use of cookies.

RPi Robot's first trick

Published 2020-01-16

Will provide more info (including schema) about the robot itself soon, for now sharing a code and movie of a simple trick.

Trick

  • Go forward until obstacle
  • Go back until obstacle
  • Try to stop in the middle between the obstacles

Code

#include <stdio.h>
#include <wiringPi.h>
#include <unistd.h>
#include <ncurses.h>
#include <termios.h>

//definicja pinow GPIO
#define L_R 19
#define R_R 13
#define L_F 20
#define R_F 26
#define B_TRIG 5
#define B_TECHO 12
#define F_TRIG 21
#define F_TECHO 16
//#define BTRIG 6
//#define BECHO 12

/**
 * warning: blocking!
 * distance pingedr
 */
static float ping( int trig, int echo )
{
    int start = micros();
    long ping = 0;
    long pong = 0;
    float distance = 0;
    long timeout = 100000; // 0.5 sec ~ 171 m

    // Trigger reset
    digitalWrite(trig, LOW);
    delay(50);

    // Trigger start
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);

    // Waiting for echo
    while (digitalRead(echo) == LOW && micros() - start < timeout)
    {
    }

    if (micros() - start > timeout)
    {
        return 9999;
    }

    ping = micros() - start;

    while (digitalRead(echo) == HIGH && micros() - start < timeout)
    {
    }

    if (micros() - start > timeout)
    {
        return 9999;
    }

    pong = micros() - start;

    // ping to distance
    distance = (float)(pong - ping) * 0.017150;
    return distance;
}

static void stop()
{
    digitalWrite(L_F, 0);
    digitalWrite(L_R, 0);
    digitalWrite(R_R, 0);
    digitalWrite(R_F, 0);
}

static void moveForward()
{
    digitalWrite(L_F, 0);
    digitalWrite(R_F, 0);
    digitalWrite(R_R, 1);
    digitalWrite(L_R, 1);
}
static void moveBack()
{
    digitalWrite(L_F, 1);
    digitalWrite(R_F, 1);
    digitalWrite(R_R, 0);
    digitalWrite(L_R, 0);
}

static void rotateLeft()
{
    digitalWrite(R_F, 0);
    digitalWrite(L_F, 1);
    digitalWrite(R_R, 1);
    digitalWrite(L_R, 0);
}

static void rotateRight()
{
    digitalWrite(R_F, 1);
    digitalWrite(L_F, 0);
    digitalWrite(R_R, 0);
    digitalWrite(L_R, 1);
}

int main(void)
{
    int i;
    /* non-blocking console mode */
    initscr();
    cbreak();
    noecho();
    scrollok(stdscr, TRUE);
    nodelay(stdscr, TRUE);

    printf("Starting\n");
    wiringPiSetupGpio();
    pinMode(L_R, OUTPUT);
    pinMode(R_R, OUTPUT);
    pinMode(L_F, OUTPUT);
    pinMode(R_F, OUTPUT);
    pinMode(F_TRIG, OUTPUT);
    pinMode(F_TECHO, INPUT);
    pinMode(B_TRIG, OUTPUT);
    pinMode(B_TECHO, INPUT);
    int pingval_front = 1000;
    int pingval_back = 1000;

    //going forward
    moveForward();
    int hittime, backtime, total, traveltime;
    while ( ping (F_TRIG, F_TECHO) > 8 ) { } //going forward until less than 8cm
    hittime = micros(); //hit time
    moveBack();
    while ( ping (B_TRIG, B_TECHO) > 8 ) { } //going backwards until less than 8cm
    backtime = micros();
    total = backtime - hittime;
    traveltime = total / 2; //we want to travel that long
    moveForward();
    while ( micros() - backtime < traveltime ) {}
    stop();
    return 0;
}

Compilation

gcc -Wall -o run run.c -lwiringPi -lncurses

Video

Warning: content under this box is not visible due to your cookies settings as requires to load contents from YouTube. If you believe that you have made a wrong choice then please follow this link to reset your cookies settings and accept 3rd party cookies:

reset cookies settings

Photos

Robot 1 Robot 2 Robot 3

Summary

More info soon, will provide robot's schema.

Need to fix a bit the mounting of tracks as robot seems to be turning left a bit.