Arduino - Drawing Route for Car on the Office's Map

Demo

Thing Used in This Project

1 x Arduino Uno or Mega
1 x PHPoC Shield R2 or PHPoC WiFi Shield R2
2 x Stepper Motor Controller PES-2605
Step Motor Car http://www.smartarduino.com/view.php?id=94504
A limit switch.
Battery for Arduino
Battery for step motor
Jumper wires

The Idea

The main idea was to control a step motor-based robot by drawing route on web-based map, making the robot carry your goods to destination just by touching your finger on mobile screen.

In this project, I make a simple car without using feedback sensor. Therefore, position error is accumulated over time.

This project may inspire someone to make a perfect carrier robot by using some kind of feedback sensors to correct position error. This will be more convenient if it is combined with a routing algorithm such as Dijkstra algorithm.

Wiring

Stack PHPoC Shield or PHPoC WiFi Shield on Arduino
Stack Two Stepper Motor Controllers PES-2604 on PHPoC Shield or PHPoC WiFi Shield
Connect stepper motors to terminal block of Stepper Motor Controller PES-2605
- Bipolar stepper motor
- Unipolar stepper motor: there are two ways to connect a unipolar stepper motor to terminal block of PES-2605. User can choose one of them.
Set expansion ID for each step motor controllers are 13 and 14 via DIP switch on expansion board.
You can refer to below table to set expansion ID.
Connect limit switch to SW#0 of digital input port of both step motor controllers

How It Works

We need an image of office's map. The map's size must have the same aspect ratio as real size.

Tip: drawing map will be more exact in term of aspect ratio if it is drawn based on then floor bricks.

Map image is scaled to fit the mobile screen. When finger touch on web-based map:

- The XY coordinate (in pixel) is rescaled to image's coordinate, and then is sent to Arduino as target position.
Arduino converts the pixel coordinate to real coordinate (in millimeter).

Code:

/* convert pixel to millimeter */
targetX = (long)(targetX / RATIO_PIX_PER_MM);
targetY = (long)(targetY / RATIO_PIX_PER_MM);

- Arduino calculates the direction (rotated angle) and distance based on: current position, previous direction and target position

Code:

/* calculate the rotating angle */
vectorX  = targetX - preX;
vectorY  = targetY - preY;

dist = sqrt(vectorX * vectorX + vectorY * vectorY);

vectorX /= dist;
vectorY /= dist;

cosinAngle = (preVectorX * vectorX + preVectorY * vectorY); /* based on dot product between two vectors */
angle = acos(cosinAngle) * 180 / PI;

- Arduino converts the rotated angle to number of step the motor has to move and send command to step motor controller to move the motor.

Code:

stepMove = (angle * TRACK) / ((FULL_STEP_ANGLE / stepMode) * WHEEL_RADIUS) / 2;
dir = preVectorX * vectorY - preVectorY * vectorX; /* direction of rotation: clockwise or counter-clockwise */

WHEEL_RADIUS is radius of wheel.

- Arduino converts the distance to number of step the motor has to move and send command to step motor controller to move the motor.

Code:

stepMove = dist / WHEEL_PERIMETER * (FULL_STEP_NUM * stepMode);

targetStepRight += stepMove;
targetStepLeft -= stepMove;

WHEEL_PERIMETER is perimeter of wheel

Limit Switch

A limit switch is placed at the head of car to make motor stop automatically when meeting obstacles.

Initial Position of Car
Initial position is position of car when Arduino boots or resets. It must be the same among: real position (where you put your car), position setting in Arduino Code and Web User Interface code

In my setting, I set my car at position (16th brick, 22th brick) (see below image)

In Arduino code (line 54, 55)

//initial position
preX = 16 * BRICK_REAL_WIDTH;
preY = 22 * BRICK_REAL_HEIGHT;

In Web User Interface (line 47. 48)
//initial position
//initial position
touchImageX = 16 * IMAGE_WIDTH / 25.0;
touchImageY = 22 * IMAGE_HEIGHT / 34.0;

Source Codes

Arduino Code

This is arduino code, which run in infinite loop.

Code:

#include <Phpoc.h>
#include <PhpocExpansion.h>
#include <math.h>

#define TRACK                191 //in mm
#define WHEEL_RADIUS        (double)48.2 // in mm
#define WHEEL_PERIMETER        (double)(2*M_PI*WHEEL_RADIUS) // in mm

#define FULL_STEP_ANGLE        1.8 //degree
#define FULL_STEP_NUM        200 //step per round

#define MAP_IMG_WIDTH        1140 //pixel
#define BRICK_REAL_WIDTH    (double)450
#define BRICK_REAL_HEIGHT    (double)450
#define MAP_REAL_WIDTH        (double)(25 * BRICK_REAL_WIDTH) //millimeter
#define RATIO_PIX_PER_MM     (double)(MAP_IMG_WIDTH / MAP_REAL_WIDTH) //pixel / millimeter

#define STATE_FREE        0
#define STATE_PAUSE        1
#define STATE_MOVE        2

PhpocServer server(80);
SppcStepper stepRight(13);
SppcStepper stepLeft(14);
int stepMode;

double preX, preY;
double preVectorX, preVectorY ;
double targetStepRight, targetStepLeft;
byte carState;
int limitSwitchPin = 0;

void carInit() {
    stepMode = 32; /* half-step */

    stepRight.setVrefStop(2);
    stepRight.setVrefDrive(14);
    stepRight.setMode(stepMode);
    stepRight.setResonance(120, 250);
    stepRight.setSpeed(300 * stepMode);
    stepRight.setAccel(800 * stepMode, 800 * stepMode);

    stepLeft.setVrefStop(2);
    stepLeft.setVrefDrive(14);
    stepLeft.setMode(stepMode);
    stepLeft.setResonance(120, 250);
    stepLeft.setSpeed(300 * stepMode);
    stepLeft.setAccel(800 * stepMode, 800 * stepMode);

    stepRight.setEioMode(limitSwitchPin, EIO_MODE_LOCK);
    stepLeft.setEioMode(limitSwitchPin, EIO_MODE_LOCK);

    //initial position
    preX = 16 * BRICK_REAL_WIDTH;
    preY = 22 * BRICK_REAL_HEIGHT;
    preVectorX = 0;
    preVectorY = 1;

    targetStepRight = 0.0;
    targetStepLeft = 0.0;

    carState = STATE_FREE;
}

void networkLoop() {
    PhpocClient client = server.available();

    if(client) {
        if(carState != STATE_FREE)
            return;

         String data = client.readLine();

        if(data) {
            int commaPos;
            long targetX, targetY;

            commaPos = data.indexOf(',');
            targetX = data.substring(0, commaPos).toInt();
            targetY = data.substring(commaPos + 1).toInt();

            /* convert pixel to millimeter */
            targetX = (long)(targetX / RATIO_PIX_PER_MM);
            targetY = (long)(targetY / RATIO_PIX_PER_MM);

            carGoto(targetX, targetY);
        }
    }
}
/*
* This function is used to check the obstacle.
* A limit switch is installed in front of car.
* If limit switch is touched is detected (due to obstacle), step motor controller (PES-2605 will pause step motor automatically (step motor is set to the locked state).
* This function checks whether obstacle is removed or not. If removed, it unlocks motor to make motor continue to move.
*/
void carLoop() {
    if(carState == STATE_FREE)
        return;

    if(stepRight.getState() == 1 || stepLeft.getState() == 1) {
        carState = STATE_PAUSE;

        if(stepRight.getEio(limitSwitchPin) == 1 && stepRight.getEio(limitSwitchPin) == 1) {
            stepRight.unlock();
            stepLeft.unlock();

            stepRight.setEioMode(limitSwitchPin, EIO_MODE_LOCK);
            stepLeft.setEioMode(limitSwitchPin, EIO_MODE_LOCK);

            stepRight.stepGoto((long)targetStepRight);
            stepLeft.stepGoto((long)targetStepLeft);
            carState = STATE_MOVE;
        }
    }

    long currentStep1, currentStep2;

    currentStep1 = stepRight.getPosition();
    currentStep2 = stepLeft.getPosition();

    if(currentStep1 == (long)targetStepRight && currentStep2 == (long)targetStepLeft)
        carState = STATE_FREE;
}
void carGoto(long targetX, long targetY) {
    double vectorX, vectorY;
    double cosinAngle, angle, dir;
    double dist, stepMove;

    /* calculate the rotating angle */
    vectorX  = targetX - preX;
    vectorY  = targetY - preY;

    dist = sqrt(vectorX * vectorX + vectorY * vectorY);

    vectorX /= dist;
    vectorY /= dist;

    cosinAngle = (preVectorX * vectorX + preVectorY * vectorY); /* based on dot product between two vectors */
    angle = acos(cosinAngle) * 180 / PI;
    stepMove = (angle * TRACK) / ((FULL_STEP_ANGLE / stepMode) * WHEEL_RADIUS) / 2;
    dir = preVectorX * vectorY - preVectorY * vectorX; /* direction of rotation: clockwise or counter-clockwise */

    if(dir < 0) {
        targetStepRight += stepMove;
        targetStepLeft += stepMove;
    } else {
        targetStepRight -= stepMove;
        targetStepLeft -= stepMove;
    }

    /* rotate car */
    stepRight.stepGoto((long)targetStepRight);
    stepLeft.stepGoto((long)targetStepLeft);

    while(stepRight.getState())
        ;
    while(stepLeft.getState())
        ;

    /* convert millimeter to step */
    stepMove = dist / WHEEL_PERIMETER * (FULL_STEP_NUM * stepMode);

    targetStepRight += stepMove;
    targetStepLeft -= stepMove;

    /* after rotating, move car straight */
    stepRight.stepGoto((long)targetStepRight);
    stepLeft.stepGoto((long)targetStepLeft);

    carState = STATE_MOVE;

    preX = targetX;
    preY = targetY;
    preVectorX = vectorX;
    preVectorY = vectorY;
}

void setup() {
    Serial.begin(9600);
    while(!Serial)
        ;

    Phpoc.begin(PF_LOG_SPI | PF_LOG_NET);
    //Phpoc.begin();

    server.beginWebSocket("remote_car");

    Serial.print("WebSocket server address : ");
    Serial.println(Phpoc.localIP());

    Sppc.begin(PF_LOG_SPI | PF_LOG_NET);
    Expansion.begin();

    carInit();
}

void loop() {
    carLoop();
    networkLoop();
}

Web User Interface - remote_car.php

remote_car.php is a file that contains Web User Interface. It needs to be stored on PHPoC [WiFi] Shield. In order to upload the file to PHPoC [WiFi] Shield, please do the following steps:

Copy the below code and save it into remote_car.php file.
Install PHPoC Debugger
Connect PHPoC Debugger to PHPoC [WiFi] Shield via micro-USB cable according to this instruction.
Note that Arduino must be powered.
Upload remote_car.php file to PHPoC [WiFi] Shield according to this instruction
Upload map.png to to PHPoC [WiFi] Shield

PHP Code:


<!DOCTYPE html>
<html>
<head>
<title>Arduino - PHPoC Shield - Step Motor</title>
<meta name="viewport" content="width=device-width, initial-scale=0.7">
<style>
body {
    text-align: center;
    background-color: #00979D;
}
#canvas {
    margin-right: auto;
    margin-left: auto;
    position: relative;
}
canvas {
    position: absolute;
    left: 0px;
    top: 0px;
    overflow-touchImageY: auto;
    overflow-touchImageX: hidden;
    -webkit-overflow-scrolling: touch; /* nice webkit native scroll */
}
#layer1 { z-index: 2; }
#layer2 {
    z-index: 1;
    background: url(map.png) no-repeat;
    background-size: contain;
    border: 1px solid #000;
}
</style>
<script>
var RESOLUTION = 100;
var IMAGE_WIDTH = 1140, IMAGE_HEIGHT = 1562;

var ws = null;
var layer1 = null, layer2 = null;
var canvas = null;
var ctx1 = null, ctx2 = null;

var canvasWidth, canvasHeight;
var touchImageX, touchImageY; //position of car on map's image coordinate (pixel)
var touchState = 0;

function init() {
    //initial position
    touchImageX = 16 * IMAGE_WIDTH / 25.0;
    touchImageY = 22 * IMAGE_HEIGHT / 34.0;

    canvas = document.getElementById("canvas");

    layer1 = document.getElementById("layer1");
    layer2 = document.getElementById("layer2");

    layer1.addEventListener("touchstart", mouse_down);
    layer1.addEventListener("touchend", mouse_up);
    layer1.addEventListener("touchmove", mouse_move);
    layer1.addEventListener("mousedown", mouse_down);
    layer1.addEventListener("mouseup", mouse_up);
    layer1.addEventListener("mousemove", mouse_move);

    ctx1 = layer1.getContext("2d");
    ctx2 = layer2.getContext("2d");

    canvas_resize();
}
function ws_onmessage(e_msg) {
    var arr = JSON.parse(e_msg.data);
    var carPosStepX            = arr[0];
    var carPosStepY            = arr[1];

    var carPosPixelX = Math.round(carPosStepX * canvasWidth / MAX_X);
    var carPosPixelY = -Math.round(carPosStepY * canvasHeight / MAX_Y);

    ctx1.clearRect(0, 0, canvasWidth, -canvasHeight);
    ctx1.beginPath();
    ctx1.arc(carPosPixelX, carPosPixelY, 7, 0, 2*Math.PI);
    ctx1.fill();
}
function ws_onopen() {
    document.getElementById("ws_state").innerHTML = "OPEN";
    document.getElementById("wc_conn").innerHTML = "Disconnect";
}
function ws_onclose() {
    document.getElementById("ws_state").innerHTML = "CLOSED";
    document.getElementById("wc_conn").innerHTML = "Connect";
    ws.onopen = null;
    ws.onclose = null;
    ws.onmessage = null;
    ws = null;
}
function wc_onclick() {
    if(ws == null) {
        ws = new WebSocket("ws://<?echo _SERVER("HTTP_HOST")?>/remote_car", "text.phpoc");
        document.getElementById("ws_state").innerHTML = "CONNECTING";

        ws.onopen = ws_onopen;
        ws.onclose = ws_onclose;
        ws.onmessage = ws_onmessage;  
    } else {
        ws.close();
    }
}
function event_handler(event, type) {
    var touchScreenX, touchScreenY; //position in canvas coordinate (in pixel).

    if(event.targetTouches) {
        if(event.targetTouches.length > 1)
            return;

        var targetTouches = event.targetTouches;

        touchScreenX = targetTouches[0].pageX - canvas.offsetLeft;
        touchScreenY = targetTouches[0].pageY - canvas.offsetTop - canvasHeight;
    } else {
        touchScreenX = event.offsetX;
        touchScreenY = event.offsetY - canvasHeight;
    }

    // convert from screen coordinate to map's image coordinate
    var newTouchImageX = Math.round(touchScreenX / canvasWidth * IMAGE_WIDTH);
    var newTouchImageY = Math.round((-touchScreenY) / canvasHeight * IMAGE_HEIGHT);

    if(type == "MOVE") {
        var deltaX = newTouchImageX - touchImageX;
        var deltaY = newTouchImageY - touchImageY;
        var dist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));

        if(dist < RESOLUTION)
            return;
    }

    touchImageX = newTouchImageX;
    touchImageY = newTouchImageY;

    if(ws != null && ws.readyState == 1) {
        ws.send(touchImageX + "," + touchImageY + "\r\n");
        console.log(touchImageX + "," + touchImageY);

        ctx2.lineTo(touchScreenX, touchScreenY);
        ctx2.stroke();
    }
}
function mouse_down() {
    event.preventDefault();
    event_handler(event, "DOWN");
    touchState = 1;
}
function mouse_up() {
    event.preventDefault();
    touchState = 0;
}
function mouse_move() {
    event.preventDefault();
    if(touchState == 1)
        event_handler(event, "MOVE");
}
function canvas_clear() {
    ctx1.clearRect(0, 0, canvasWidth, -canvasHeight);
    ctx2.clearRect(0, 0, canvasWidth, -canvasHeight);
}
function canvas_resize() {
    var width = window.innerWidth;
    var height = window.innerHeight;
    canvasHeight = height - 100;
    canvasWidth = Math.round(canvasHeight / IMAGE_HEIGHT * IMAGE_WIDTH);

    canvas.style.width = canvasWidth + "px";
    canvas.style.height = canvasHeight + "px";
    layer1.width = canvasWidth;
    layer1.height = canvasHeight;
    layer2.width = canvasWidth;
    layer2.height = canvasHeight;

    ctx1.translate(0, canvasHeight);
    ctx1.lineWidth = 5;
    ctx1.fillStyle = "green";
    ctx2.translate(0, canvasHeight);
    ctx2.lineWidth = 5;
    ctx2.strokeStyle = "blue";

    var touchScreenX = Math.round(touchImageX * canvasWidth / IMAGE_WIDTH);
    var touchScreenY = Math.round((-touchImageY) * canvasHeight / IMAGE_HEIGHT);

    ctx1.beginPath();
    ctx1.arc(touchScreenX, touchScreenY, 3, 0, 2*Math.PI);
    ctx1.fill();
    ctx2.beginPath();
    ctx2.lineTo(touchScreenX, touchScreenY);
}

window.onload = init;
</script>
</head>

<body onresize="canvas_resize()">
<div id="canvas">
    <canvas id="layer1"></canvas>
    <canvas id="layer2"></canvas>
</div>
<p>WebSocket : <span id="ws_state">null</span></p>
<button id="wc_conn" type="button" onclick="wc_onclick();">Connect</button>
<button type="button" onclick="canvas_clear();">Clear</button>
</body>
</html>

How To

Config network information for PHPoC shield or PHPoC WiFi shield
Install PHPoC Library
Install PHPoC Expansion Library
Compile and upload code to Arduino
Upload web user interface to PHPoC [WiFi] shield
Open Serial Monitor and copy IP address of PHPoC Shield
Access Web User Interface via Web Browser: http://ip_address_of_shield/remote_car.php
Control car by point position on the map

Announcement

Arduino - Drawing Route for Car on the Office's Map

Arduino - Drawing Route for Car on the Office's Map