Thursday, July 3, 2014

Prototyping an autonomous robot with Javascript

You may be asking yourself; why would I choose Javascript for the first prototype of our autonomous robot?  The answer is really simple, I wanted to see if I could write it in Javascript and I also wondered how effective Javascript would be at controlling an autonomous robot.  First, lets see a video of our prototype in action:





If you are not familiar with using Javascript/Bonescript with the BeagleBone Black, you will probably want to take a look at my previous post Using Javascript with Bonescript to program the BeagleBone Black before reading this post.  The My firstworking robot, it’s alive post details how we built the robot and the LV-MaxSonar-EZ2 Range Finder post shows how we connected the LV-MaxSonar-EZ2 Range Finder to our robot.

In the My first working robot, it’s alive – Part 2 post, I wrote a python module that defined the basic movements of robot like changing speeds, changing direction, going forward and stop.  In this post, the first thing we will do is to write a similar Javascript module that we can load with Node.js.  Here is the Javascript code for this module:

var b = require('bonescript')

var PIN_SPEED_RIGHT = "P8_13";
var PIN_SPEED_LEFT = "P8_19";
var PIN_DIR_LEFT = "P8_14";
var PIN_DIR_RIGHT = "P8_16";
var MAX_SPEED=1;
var MIN_SPEED=.25;
var CHANGE_RATE=.05;
var STOP_SPEED=0;
var FORWARD_DIR=1;
var REVERSE_DIR=0;

var current_speed_right = STOP_SPEED;
var current_speed_left = STOP_SPEED;
var current_dir_right = FORWARD_DIR;
var current_dir_left = FORWARD_DIR;

//initiate rover
function initRover() {
       b.pinMode(PIN_DIR_LEFT,b.OUTPUT);
       b.pinMode(PIN_DIR_RIGHT, b.OUTPUT);
      
}
exports.initRover = initRover;


//Utility rover to check if the speed is within range
function checkSpeed(speed) {
       if (speed < MIN_SPEED && speed != STOP_SPEED)
              speed = MIN_SPEED;
      
       if (speed > MAX_SPEED)
              speed = MAX_SPEED;
      
       return speed;
}

//Utility sleep
function sleep(milliseconds) {
       var currentTime = new Date().getTime();

  while (currentTime + milliseconds >= new Date().getTime()) {
  }
}
exports.sleep = sleep;

//Set the speed of the tracks
function setRightSpeed(speed) {
       var newSpeed = checkSpeed(speed);
       b.analogWrite(PIN_SPEED_RIGHT, newSpeed);
       current_speed_right = newSpeed;
}
exports.setRightSpeed = setRightSpeed;

function setLeftSpeed(speed) {
       var newSpeed = checkSpeed(speed);
       b.analogWrite(PIN_SPEED_LEFT, newSpeed);
       current_speed_left = newSpeed;
}
exports.setLeftSpeed = setLeftSpeed;

function setSpeed(speed) {
       setLeftSpeed(speed);
       setRightSpeed(speed);
}
exports.setSpeed = setSpeed;

//Increase speed
function increaseRightSpeed() {
       setRightSpeed(current_speed_right + CHANGE_RATE);
}
exports.increaseRightSpeed = increaseRightSpeed;

function increaseLeftSpeed() {
       setLeftSpeed(current_speed_left + CHANGE_RATE);
}
exports.increaseLeftSpeed = increaseLeftSpeed;

function increaseSpeed() {
      
       increaseLeftSpeed();
       increaseRightSpeed();
}
exports.increaseSpeed = increaseSpeed;

//Decrease Speed
function decreaseRightSpeed() {
       setRightSpeed(current_speed_right - CHANGE_RATE);
}
exports.decreaseRightSpeed = decreaseRightSpeed;

function decreaseLeftSpeed() {
       setLeftSpeed(current_speed_left - CHANGE_RATE);
}
exports.decreaseLeftSpeed = decreaseLeftSpeed;

function decreaseSpeed() {
       decreaseLeftSpeed();
       decreaseRightSpeed();
}
exports.decreaseSpeed = decreaseSpeed;

//set direction forward
function forwardRightDirection() {
       if (current_dir_right == REVERSE_DIR)
              allStop();
       b.digitalWrite(PIN_DIR_RIGHT, b.HIGH);
       current_dir_right = FORWARD_DIR;
}
exports.forwardRightDirection = forwardRightDirection;

function forwardLeftDirection() {
       if (current_dir_left == REVERSE_DIR)
              allStop();
       b.digitalWrite(PIN_DIR_LEFT, b.HIGH);
       current_dir_left = FORWARD_DIR;
}
exports.forwardLeftDirection = forwardLeftDirection;

function forwardDirection() {
       forwardLeftDirection();
       forwardRightDirection();
}
exports.forwardDirection = forwardDirection;

//set direction reverse
function reverseRightDirection() {
       if (current_dir_right == FORWARD_DIR)
              allStop();
       b.digitalWrite(PIN_DIR_RIGHT, b.LOW);
       current_dir_right = REVERSE_DIR;
}
exports.reverseRightDirection = reverseRightDirection;

function reverseLeftDirection() {
       if (current_dir_left == FORWARD_DIR)
              allStop();
       b.digitalWrite(PIN_DIR_LEFT, b.LOW);
       current_dir_left = REVERSE_DIR;
}
exports.reverseLeftDirection = reverseLeftDirection;

function reverseDirection() {
       reverseLeftDirection();
       reverseRightDirection();
}
exports.reverseDirection = reverseDirection;

//Stop rover
function stopLeft() {
       setLeftSpeed(STOP_SPEED);
}
exports.stopLeft = stopLeft;

function stopRight() {
       setRightSpeed(STOP_SPEED);
}
exports.stopRight = stopRight;

function allStop() {
       stopLeft();
       stopRight();
}
exports.allStop = allStop;

//Full speed
function fullSpeedLeft() {
       setLeftSpeed(MAX_SPEED);
}
exports.fullSpeedLeft = fullSpeedLeft;

function fullSpeedRight() {
       setRightSpeed(MAX_SPEED);
}
exports.fullSpeedRight = fullSpeedRight;

function fullSpeed() {
       fullSpeedLeft();
       fullSpeedRight();
}
exports.fullSpeed = fullSpeed;

//spin robot
function spinRoverLeft(speed) {
       allStop();
  forwardDirection();
  forwardRightDirection();
  reverseLeftDirection();
  setRightSpeed(speed);
  setLeftSpeed(speed);
}
exports.spinRoverLeft = spinRoverLeft;

function spinRoverRight(speed) {
       allStop();
  forwardDirection();
  forwardLeftDirection();
  reverseRightDirection();
  setLeftSpeed(speed);
  setRightSpeed(speed);
}
exports.spinRoverRight = spinRoverRight;

This module exposes several functions, these are:

stop_rover():  Stops the rover
check_speed(speed):  Verifies that the speed is within the acceptable ranges.  This function returns the speed that was passed in if it was within the acceptable range otherwise it returns the MAX_SPEED or MIN_SPEED depending on if the speed that was passed in was too high or too low.
sleep():  Pauses the execution of the script for a specified amount of time.

set_right_speed():  Sets the speed of the right track.
set_left_speed():  Sets the speed of the left track.
set_speed():  Sets the speed of both tracks.

increase_right_speed():  Increases the speed of the right track.
increase_left_speed():  Increases the speed of the left track.
increase_speed():  increases the speed of both tracks.

decrease_right_speed():  Decreases the speed of the right track.
decrease_left_speed():  Decrease the speed of the left track.
decrease_speed():  Decrease the speed of both tracks.

forward_right_dir():  Sets the direction of the right track to forward.
forward_left_dir():  Sets the direction of the left track to forward.
forward_dir():  Sets the direction of both tracks to forward.

reverse_right_dir():  Sets the direction of the right track to reverse.
reverse_left_dir():  Sets the direction of the left track to reverse.
reverse_dir():  Sets the direction of both tracks to reverse.

stop_left():  Stops the left track.
stop_right():  Stops the right track.
all_stop():  Stops both tracks.

full_speed_left():  Sets the left track to full speed.
full_speed_right():  Sets the right track to full speed.
all_full_speed():  Sets both tracks to full speed.

spin_right(speed):  Spins the robot in the right direction at the speed passed in.
spin_left(speed):  Spins the robot in the left direction at the speed passed in.

Now lets take a look at the code that will control our robot.  This has very basic and simple logic for our first prototype.  The robot moves forward until it is within 18 inches of an object.  Once it is within 18 inches of an object it continuous to turn right until it has over 18 inches of clearance.    Here is the code:

var b = require('bonescript');
var rover = require("./rover.js")

var ledPin = "P8_8";
var buttonPin = "P8_11";
var sensorPin = "P9_40";

var roverStateEnum = {
       INIT : "init",
       COMPLETE_STOP : "complete stop",
       STOPPED : "stopped",
       FORWARD : "moving forward",
       REVERSE : "moving reverse",
       SPIN_RIGHT : "spinning right",
       SPIN_LEFT : "spinning left"
};

var current_speed = 0;
var min_speed = .5;
var moving = roverStateEnum.COMPLETE_STOP;
var check_interval = 500;
var interval=0;
var detect_length = 18;

b.pinMode(ledPin, b.OUTPUT);
b.pinMode(buttonPin, b.INPUT);
b.attachInterrupt(buttonPin, true, b.FALLING, buttonChange);
b.digitalWrite(ledPin, b.HIGH);

function buttonChange(button) {
       console.log("Button Pressed");
       if (moving == roverStateEnum.COMPLETE_STOP || moving == roverStateEnum.STOPPED) {
              console.log("Forward");
              rover.initRover();
              rover.forwardDirection();
              current_speed = min_speed;
              rover.setSpeed(current_speed);
              if (interval == 0) {
                      console.log("setting interval");
                      setInterval(read,check_interval);
                     interval = 1;
              }
             
              moving = roverStateEnum.FORWARD;
              b.digitalWrite(ledPin, b.HIGH);
       } else {
              console.log("Stop");
              rover.allStop();
              moving = roverStateEnum.COMPLETE_STOP;
              b.digitalWrite(ledPin, b.LOW);
       }
      
}

function read() {
       b.analogRead(sensorPin,sensorStatus);
}

function sensorStatus(v) {
       var distanceInches;
       var analogVoltage = v.value*1.8;
       distanceInches = analogVoltage/0.002148;
       console.log("Object at " + parseFloat(distanceInches).toFixed(2) + " inches away");
      
       if (distanceInches < detect_length && moving != roverStateEnum.COMPLETE_STOP) {
              console.log("Stopping and spinning");
              rover.allStop();
              rover.spinRoverRight(min_speed);
              moving = roverStateEnum.SPIN_RIGHT;
              rover.sleep(500);
              rover.allStop();
              rover.sleep(500);
              moving = roverStateEnum.STOPPED;
       } else if (moving == roverStateEnum.STOPPED) {
              console.log("Going Forward");
              rover.forwardDirection();
              rover.setSpeed(current_speed);
              moving = roverStateEnum.FORWARD;
       }
}


We start off by importing our rover module and also the Bonescript module.  We then define the pins used for an LED, a button and the MaxSonar sensor.  We use the LED to show when the rover is running and the button is used to start and stop the rover.  I wired the LED and button exactly as I did in the Using Javascript with Bonescript to program the BeagleBone Black (http://myroboticadventure.blogspot.com/2014/06/using-javascript-with-bonescript-to.html) post.

We then define an enum that we will use to define what type of movement the robot is currently doing.   Next we define several variables, these are:
current_speed:  The current speed of the robot.
min_speed:  The minimum speed we want the robot to go
moving:  The current moving state of the robot
check_interval:  How often we want to check the MaxSonar sensor.  This is in milliseconds.
interval:  If 0, the robot is not checking the MaxSonar for distance, if 1 it is already checking.
detect_length:  Is the length, in inches, that will force the robot to turn.

We set the mode of the LED pin to OUTPUT which means we will be writing to the pin and we set the mode of the button pin to INPUT which means we will be reading from the pin.  We use the attachInterrupt function to call the buttonChange function every time the button is pushed.  Finally we turn on the LED, by calling the digitalWrite function, to let us know that the robot is ready to go.

The buttonChange function is called whenever the button is pressed.  If the current moving state of the robot is roverStateEnum.COMPLETE_STOP or roverStateEnum.STOPPED then we start the rover moving forward and also use the setInterval() function to begin checking the MaxSonar every half second.  If the current state is anything other than roverStateEnum.COMPLETE_STOP or roverStateEnum.STOPPED then we stop the rover.

The read function is called every half second to check the MaxSonar sensor for the current distance.  When the distance comes back from the MaxSonar sensor, it calls the sensorStatus() function.  In the sensorStatus() function, if there is an object closer than the distance defined by the detect_length variable (18 inches) and the current state of the robot is not roverStateEnum.COMPLETE_STOP, then we stop the robot and spin to the right for half a second.

This example is just the start of our autonomous robot and we currently have four more MaxSonar sensors on order so our robot can look at the world around it and decide where and how to turn.  Not sure if I want to use Javascript or Python to develop this in but will have to make the decision soon.




1 comment: