Robot Drawing on Whiteboard

17.04.17    Raspberry Pi    electronics    C++ and C

I have recently built a robot drawing on a whiteboard using a Raspberry Pi 3 and Lego-motors. I have made a video of me controlling it.

I want to present its setup and the way it is built.

Pen

The robot moves around around a standard whiteboard pen on the board to draw. The pen is inside a cardboard holder. This holder is suspended on two threads, which go through holes in the corners of the whiteboard. By pulling the threads and letting go the robot can control the position of the pen. The pen is pushed onto the board by its own weight. The robot cannot lift the pen; it can only draw continuous lines.

Motors

The robot uses two 9V DC Lego-motors. They are build into a Lego scaffolding, which is mounted on a cardboard pedestal. These motors turn each turn a coil, which hold the threads of the pen holder.

By turning the coil, the robot pulls the pen to the corresponding corner. When letting go, gravity pulls the pen in the opposite direction. When pulling one thread and unrolling the other, the pen moves on an elliptical arc (the corners of the whiteboard being the foci of the ellipse). But this movement is close enough to a straight line in this concrete setup. When pulling just one thread, the pen moves on an arc of a circle around the corner whose thread is not moving.

H Bridges: L298N

To control the motors I have used an L298N integrated circuit. It is a circuit of two H bridges, one for each motor.

Using this integrated circuit, the Raspberry Pi can control the direction of the current through the motors using the GPIO pins. This determines the movement direction. Additionally the motors can be set to brake. The current comes from six AA batteries connected to the circuit.

Joystick

In the second half of the video I control the robot using a joystick. Using an ADC circuit the Pi can read the position of the stick on the two axes. The desired speed of the motors is calculated using this input. The actual speed of the motors is controlled using a software PWM: by rapidly alternating between movement and braking signals the motor moves steadily at a slower speed.

Code

This is the code I have written to control the robot. It is a small C program which allows the user to move the pen directly or based on joystick input.

#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

//Def is for the func below
#define uchar unsigned char
#define     ADC_CS    4
#define     ADC_CLK   5
#define     ADC_DIO   6
#define  JoyStick_Z   21
uchar get_ADC_Result(uchar xyVal);

int computeSpeedL(int speedH, int speedV);
int computeSpeedR(int speedH, int speedV);
void movePen(int tick, int speedL, int speedR);
void readStickInput(int *speedH, int *speedV);

const int pin_l1 = 0;
const int pin_l2 = 1;
const int pin_r1 = 3;
const int pin_r2 = 2;
const int pulseLength = 30;
const int speedMult = 3;

This is the 'header' of the program. Some pin constants are defined using #define. These come from the code for interacting with the ADC I copied from example programs for the joystick.

int main(int argc, char** argv) {

    int speedH = 0;
    int speedV = 0;
    int time = 1000;
    int readJoystick = 0;

    int speedL = 0;
    int speedR = 0;

    if(argc == 2 && strcmp(argv[1], "joy") == 0) {
        readJoystick = 1;
    } else {
        if(argc < 3) exit(1);

        speedH = atoi(argv[1]);
        speedV = atoi(argv[2]);

        if(argc >= 4) {
            time = atoi(argv[3]);
        }
        speedH *= speedMult;
        speedV *= speedMult;

        speedL = computeSpeedL(speedH, speedV);
        speedR = computeSpeedR(speedH, speedV);
    }

This is the first part of the main function. The variables defined here control the bahaviour of the next parts. The values are set based on the command line arguments. Usage:

# draw with x-speed 100% and y-speed 80% for 1 second
$ draw 10 8

# draw with x-speed 40% and y-speed 60% for 1,5 seconds
$ draw 4 6 1500

# draw based on joystick input
$ draw joy

The following code setups the GPIO pins needed for the program.

    if(wiringPiSetup() == -1) {
        exit(1);
    }
    pinMode(pin_l1, OUTPUT);
    pinMode(pin_l2, OUTPUT);
    pinMode(pin_r1, OUTPUT);
    pinMode(pin_r2, OUTPUT);
    if(readJoystick) {
        pinMode(ADC_CS, OUTPUT);
        pinMode(ADC_CLK, OUTPUT);
        pinMode(ADC_DIO, OUTPUT);
        pinMode(JoyStick_Z, INPUT);
    }


    digitalWrite(pin_l1, 0);
    digitalWrite(pin_l2, 0);
    digitalWrite(pin_r1, 0);
    digitalWrite(pin_r2, 0);

The next code implements the software PWM:

    int tick = 0;
    while(readJoystick || tick < time) {
        if(readJoystick) {
            readStickInput(&speedH, &speedV);
            speedL = computeSpeedL(speedH, speedV);
            speedR = computeSpeedR(speedH, speedV);
        }

        movePen(tick, speedL, speedR);

        tick++;
        delay(1);
    }

This loop continues until either it has run its time in direct mode, or indefinitely in joystick mode (i.e. until the user aborts with Ctrl-c). In joystick mode the new input is read from the joystick and the new speeds are saved in the variables speedL and speedR.

The movePen sets the motor signals (move forward/backward or brake) based on the current tick and the given speeds.

    digitalWrite(pin_l1, 0);
    digitalWrite(pin_l2, 0);
    digitalWrite(pin_r1, 0);
    digitalWrite(pin_r2, 0);
    delay(200);

    return 0;
} //main

This a small shutdown part at the end of the main function. Below are the implementations of the helper functions:

void movePen(int tick, int speedL, int speedR) {
    if(speedL > 0) {
        digitalWrite(pin_l1, 0);
        digitalWrite(pin_l2, tick % pulseLength < speedL);
    } else {
        digitalWrite(pin_l2, 0);
        digitalWrite(pin_l1, tick % pulseLength < -speedL);
    }
    if(speedR > 0) {
        digitalWrite(pin_r1, 0);
        digitalWrite(pin_r2, tick % pulseLength < speedR);
    } else {
        digitalWrite(pin_r2, 0);
        digitalWrite(pin_r1, tick % pulseLength < -speedR);
    }
}

int computeSpeedL(int speedH, int speedV) {
    return speedV + speedH;
}

int computeSpeedR(int speedH, int speedV) {
    return speedV - speedH;
}

void readStickInput(int *speedH, int *speedV) {
    //rotate 90 deg
    //and negate
    int xVal = -(get_ADC_Result('y') - 127);
    int yVal = get_ADC_Result('x') - 127;

    *speedH = xVal / 11;
    *speedV = yVal / 11;
    printf("Joy %d %d\n", xVal, yVal);
}

/* Copy-Pasted ADC function */

uchar get_ADC_Result(uchar xyVal)
{
    //10:CH0
    //11:CH1
    uchar i;
    uchar dat1=0, dat2=0;

    digitalWrite(ADC_CS, 0);

    digitalWrite(ADC_CLK,0);
    digitalWrite(ADC_DIO,1);    delayMicroseconds(2);
    digitalWrite(ADC_CLK,1);    delayMicroseconds(2);
    digitalWrite(ADC_CLK,0);

    digitalWrite(ADC_DIO,1);    delayMicroseconds(2); //CH0 10
    digitalWrite(ADC_CLK,1);    delayMicroseconds(2);
    digitalWrite(ADC_CLK,0);

    if(xyVal == 'x'){
        digitalWrite(ADC_DIO,0);    delayMicroseconds(2); //CH0 0
    }
    if(xyVal == 'y'){
        digitalWrite(ADC_DIO,1);    delayMicroseconds(2); //CH1 1
    }
    digitalWrite(ADC_CLK,1);    
    digitalWrite(ADC_DIO,1);    delayMicroseconds(2);
    digitalWrite(ADC_CLK,0);    
    digitalWrite(ADC_DIO,1);    delayMicroseconds(2);

    for(i=0;i<8;i++)
    {
        digitalWrite(ADC_CLK,1);    delayMicroseconds(2);
        digitalWrite(ADC_CLK,0);    delayMicroseconds(2);

        pinMode(ADC_DIO, INPUT);
        dat1=dat1<<1 | digitalRead(ADC_DIO);
    }

    for(i=0;i<8;i++)
    {
        dat2 = dat2 | ((uchar)(digitalRead(ADC_DIO))<<i);
        digitalWrite(ADC_CLK,1);    delayMicroseconds(2);
        digitalWrite(ADC_CLK,0);    delayMicroseconds(2);
    }

    digitalWrite(ADC_CS,1);

    pinMode(ADC_DIO, OUTPUT);

    return(dat1==dat2) ? dat1 : 0;
}

The code is compiled using the following command (wiringPi needs to be installed):

gcc -o draw draw.c -lwiringPi