TMABot

The TMABot is a small robot we have built to learn more about robotics, collision avoidance and detection, path planning, etc... This document describes how you can make your own copy of TMABot and how to modify it.

• Introduction
• Functional requirements
• Technical design
• Parts
• Putting the parts together
• Electronics
• Software

This work is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License.

The TMABot is our newest robot and is developed to learn more about different kind of sensors, collision avoidance and path-planning. We built the robot with parts we had available in our lab. The Robot is build by Detlef and Rein at the MechanicApe labs. Detlef did the mechanical stuff and Rein the electronics and the software. You can always contact them via http://mechanicape.com/contact for questions about the design and ideas to improve the robot.

We have a few functional demands:

• The robot must be easy to build
• It should be easy to modify to robot
• The robot should be able to avoid obstacles and be friendly to the environment
• The robot must be connected to other devices (microcontrollers, communication, etc) to extent the possibilities

Below is a picture of all major components

Below is a list of the parts we used for the robot with supplier and estimated costs in euro's

Below is a very short description about the assembly of the robot. Use your own imagination and use whatever is available. Don't solder wires directly to components but use connectors.

1. Find a piece of flat wood (triplex, multiplex, MDF) size: 25x25cm
2. Connect the motors to the bottom side.
3. Connect the wheels and wheelhub to the motor
4. Attach the arduino to the upper side.
5. Attach the battery
6. Attach the Sonar sensors (front-left and front-right)
7. Attach the Sharp sensor (front-center)
8. Attach the Compass on top (as far from the motors as possible)
9. Attach the Microswitches to the front-left and front-right side of the wood. They will be used as bumpers
10. Connect all devices to the Arduino (see electronics diagram)

Below is a picture of the Arduino Duemilanove microcontroller with the connected devices/sensors. Please consult the datasheet of your hardware to check the pins before you connect them to the Arduino.

//@name:        TMABOT
//@version:     1.2
//@author:      Rein Velt, 
//@company:     Theo's Mechanic Ape (http://mechanicape.com)
//@description: Main software to run the TMA bot and drive the motors. 
//              The software detects collisions with the sonar sensors.
//              After collision the pantilt servo with the sharp distancesensor
//              will do a 180 degree sweep to look for the best way out
//@controller:  Arduino Duemilanove 
//@sensors:     srf02 sonar (x2),sharp 2yk sensor (x1), microswitch (x2) , compass hb55b(x1)
//@actuators:   Sparkfun Ardumoto (x1), Conrad Gearbox dc motors (x2), Modelcraft servomotor (x2), 
//Other credits:
//* SRF02 Arduino Library - (C) 2008 Dirk Grappendorf, GrapeLabs (www.grapelabs.de).
//* HB55B Hitachi Compass sensor code partly from (c) kiilo kiilo@kiilo.org



#include 
#include "Wire.h"
#include "SRF02.h"


//PIN AND HARDWARE DEVICE DEFINITIONS
const int pinDistanceSensor      =1;  //analog in to Sharp distance sensor
const int pinFrontBumperLeft     =2;  //didgital in to frontbumper microswitch
const int pinFrontBumperRight    =3;  //digital in to frontbumper microswitch
const int pinCompassDIO          =7;  //digital i/o compass data
const int pinCompassClock        =8;  //digital out compass clock
const int pinCompassEnable       =9;  //digital out compass enable line      
const int pinMotorLeftPower      =10; //digital out pwm for left motor speed
const int pinMotorRightPower     =11; //digital out pwm for right motor speed
const int pinMotorLeftDirection  =12; //digital out for left motor forward/backward
const int pinMotorRightDirection =13; //digital out for right motor forward/backward


//sensorlimits
const int SENSORSONARMIN=20;        //the minimum accurate value of the srf02 sonarsensor in cm
const int SENSORSONARMAX=200;       //the maximum accurate value of the srf02 sonarsensor in cm
const int SENSORSHARPMIN=24;        //the minimum accurate value of the sharp yk2 distancesensor in cm
const int SENSORSHARPMAX=200;       //the maximum accurate value of the sharp yk2 distancesensor in cm
const int MOTORMINSPEED=0;          //minimum motor speed (in %)
const int MOTORMAXSPEED=100;        //maximum motor speed (in %)
const int MOTORDEFSPEED=20;         //default (safe) speed. Lower number = more accuracy (but slower)

//collision detect limits
const int COLLISSIONRANGEMIN=70;    //the maximum distance in cm to enter collission state
const int COLLISSIONRANGEMAX=100;   //the minimum distance in cm to exit collission state

//OTHER CONSTANT DEFINITIONS (MAGIC NUMBERS)
const int DEVICE_ID           =11;
const int MOTOR_FORWARD       =LOW;
const int MOTOR_BACKWARD      =HIGH;
const int DIRECTION_UNDEFINED =0;
const int DIRECTION_FORWARD   =1;
const int DIRECTION_BACKWARD  =2;
const int DIRECTION_TURNLEFT  =3;
const int DIRECTION_TURNRIGHT =4;
const int STATE_ERROR         =-1;
const int STATE_IDLE          =0;
const int STATE_FRONTBUMPER   =1;
const int STATE_BACKWARD      =2;
const int STATE_COLLISSION    =3;
const int STATE_POSTCOLLISSION=4;
const int STATE_DRIVE         =5;
const int STATE_ADJUSTCOURSE  =6;
const int STATE_COMMAND       =7;


//global variable parameters
int _debug              =0;
int _state              =STATE_IDLE;
int _stateNext          =STATE_IDLE;
int _motorDirection     =DIRECTION_UNDEFINED;
int _motorSpeed         =0;
int _sequence           =0;
int _distanceLeft       =0;
int _distanceRight      =0;
int _distanceCenter     =0;
int _compassAngle       =0;
int _delaytime          =0;
int _commandDirection   =0;




//SRF02 sonar globals (the sonar will be controlled by I2C protocol)
SRF02 _sonarRight  =SRF02(0x70, SRF02_CENTIMETERS); //initalizee left sonar (address 0x71, distance in cm)
SRF02 _sonarLeft =SRF02(0x71, SRF02_CENTIMETERS); //initialize right sonar (address 0x70, distance in cm)



void setup() 
{
   initVariables();
   initFrontBumpers();
   initSonars();
   initCompass();
   initSerialPort();
   delay(1000);              //wait 1 second before program starts
}








void loop()
{
  
  _sequence++;  
  if (_sequence>255) { _sequence=0;}
  if (Serial.available()>1) { _state=STATE_COMMAND; }
  updateSharpSensor();
  updateSonars();
  updateCompass();
  delay(_delaytime);
  updateFrontBumper(); 
  
  
  switch (_state)
  {
    
  
  
  
  
  case STATE_DRIVE:
    {      
      //drive (move) state
      _delaytime=40;
      _motorSpeed=MOTORMAXSPEED;
      _motorDirection=DIRECTION_FORWARD;
      setMotors(_motorSpeed,_motorDirection);
      //escape the drive state under the following conditions
      if ((abs(_distanceLeft-_distanceRight)>round(SENSORSONARMAX-(SENSORSONARMIN*2))))
      {
         _stateNext=STATE_ADJUSTCOURSE; 
      }
      if (( (_distanceLeft<_distanceRight)
      {
        _motorDirection=DIRECTION_TURNRIGHT;
      }
      else
      {
        _motorDirection=DIRECTION_TURNLEFT; 
      }
      
       setMotors(_motorSpeed,_motorDirection);
       //escape the collission state under the following conditions
      //updateDistanceSweeper();
      _stateNext=STATE_POSTCOLLISSION;
    }
    break;

  case STATE_POSTCOLLISSION:
    {
      //the post collision state causes the robot slowly spin around its own axis
      //until there is free space detected by the sonar sensors
      //post collission messes with the servo...so reset it to default
      _delaytime=0;
      _motorSpeed=MOTORDEFSPEED;
       setMotors(_motorSpeed,_motorDirection);
       delay(150);
       setMotors(0,0);
       //delay(100);
      //escape the postcollission state under the following conditions
      if (((_distanceLeft>COLLISSIONRANGEMAX) && (_distanceRight>COLLISSIONRANGEMAX)) && (_distanceCenter>COLLISSIONRANGEMAX))
      {
        _stateNext=STATE_DRIVE;
      }
      
    }
    break;

  case STATE_BACKWARD:
    //backward state
    { 
      _delaytime=100;
      _motorDirection=DIRECTION_BACKWARD;
      _motorSpeed=MOTORDEFSPEED; 
       setMotors(_motorSpeed,_motorDirection);
       //escape the backward state under the following conditions
       if (_distanceLeft>(SENSORSONARMIN*2)   && _distanceRight>(SENSORSONARMIN*2))
       {
        _stateNext=STATE_COLLISSION;
       }   
      

    }
    break;

  case STATE_FRONTBUMPER:
    { 
      _delaytime=250;
      int buttonStateL=digitalRead(pinFrontBumperLeft);
      int buttonStateR=digitalRead(pinFrontBumperRight);
      if (buttonStateL<1 && buttonStateR<1)
      {
         _stateNext=STATE_FRONTBUMPER;
         _motorDirection=DIRECTION_BACKWARD;
         _motorSpeed=MOTORMAXSPEED; 
      }
      else
      {
          //left switch -> turn left
          if (buttonStateL<1 && buttonStateR>0)
          {
             _stateNext=STATE_FRONTBUMPER;
             _motorDirection=DIRECTION_TURNRIGHT;
             _motorSpeed=MOTORMAXSPEED; 
          }
          
          //right switch -> turn right
          if (buttonStateL>0 && buttonStateR<1)
          {
              _stateNext=STATE_FRONTBUMPER;
              _motorDirection=DIRECTION_TURNLEFT;
              _motorSpeed=MOTORMAXSPEED; 
          }
          
          //both switch -> turn backward
          if (buttonStateL>0 && buttonStateR>0)
          {
              _stateNext=STATE_FRONTBUMPER;
              _motorDirection=DIRECTION_BACKWARD;
              _motorSpeed=MOTORMAXSPEED; 
          }
          
          
        
      }
    
       setMotors(_motorSpeed,_motorDirection);
      //always to to backward state (were we came from)
      _stateNext=STATE_BACKWARD;           
    }
    break; 
    
    
  case STATE_ADJUSTCOURSE:
    {
      _delaytime=10;
      _motorSpeed=MOTORDEFSPEED;
        int avg=round((_distanceLeft+_distanceRight+_distanceCenter)/3);
        int avgl=round((_distanceLeft+_distanceCenter)/2);
        int avgr=round((_distanceCenter+_distanceRight)/2);
        if (avgl>avgr)
        {
          _motorDirection=DIRECTION_TURNRIGHT;
        }
        else
        {
          _motorDirection=DIRECTION_TURNLEFT;
        }
       setMotors(_motorSpeed,_motorDirection);
      _stateNext=STATE_DRIVE; 
    }
    break;
  
  case STATE_COMMAND:
    {  
      //we dont like undefined states...and we want to be safe.
      //so shut down everything
      if (Serial.available()>1)
      {
         int directionHi=Serial.read();
         int directionLo=Serial.read();
         int direction=(directionHi*255)+directionLo;
         if (abs(direction-_compassAngle)>15)
         {
           if (direction>_compassAngle)
           {
             _motorDirection=DIRECTION_TURNRIGHT; 
           }
           else
           {
             _motorDirection=DIRECTION_TURNLEFT;
           }
         }
         else
         {
            _motorDirection=DIRECTION_FORWARD;
         }
         _motorSpeed=MOTORDEFSPEED;
         _stateNext=STATE_COMMAND;
      }
      else
      {
         _stateNext=STATE_DRIVE; 
      }
      setMotors(_motorSpeed,_motorDirection);
    break;
    
  }
  
  case STATE_IDLE:
  default:
    {  
      //we dont like undefined states...and we want to be safe.
      //so shut down everything
      _delaytime=0;
      _stateNext=STATE_IDLE;
      _motorSpeed=MOTORDEFSPEED;
      _motorDirection=DIRECTION_TURNLEFT;
       setMotors(_motorSpeed,_motorDirection);
       delay(100);
       setMotors(0,0);
       delay(400);
    }
    break;
  }



  //send parameter to xbee
  sendParameters();
  
  //go to next state
  _state=_stateNext;

  
}




  








void initVariables()
{
  _state=STATE_IDLE;
  _sequence=0;
  _motorSpeed=MOTORMINSPEED;
  _motorDirection=DIRECTION_UNDEFINED;
  _distanceRight=0;
  _distanceLeft=0;
  _distanceCenter=0;
}

void updateSharpSensor()
{
  _distanceCenter=getSharpDistance();
   if (_distanceCenter>SENSORSHARPMAX) { _distanceCenter=255; }
}

void updateSonars()
{
    _distanceLeft=_sonarLeft.read();
    _distanceRight=_sonarRight.read();
    if (_distanceLeft>SENSORSONARMAX) { _distanceLeft=255; }
    if (_distanceRight>SENSORSONARMAX) { _distanceRight=255; }
    SRF02::update();
    
}

void initCompass()
{
   pinMode(pinCompassEnable,OUTPUT);
   pinMode(pinCompassClock,OUTPUT);
   pinMode(pinCompassDIO,INPUT);
   compassReset();
   _compassAngle=999;
  
}

void updateCompass()
{
 
    compassReadCommand(); // read data and print Status 
    int X_Data = ShiftIn(11); // Field strength in X
    int Y_Data = ShiftIn(11); // and Y direction
    //Serial.print (X_Data);
    //Serial.print(' ');
    //Serial.print(Y_Data);
    digitalWrite(pinCompassEnable, HIGH); // ok deselect chip
    _compassAngle= abs(180+(180 * (atan2(-1 * Y_Data , X_Data) / M_PI))); // angle is atan( -y/x) !!!
   compassStartAcquisition(); // necessary!!
    //delay(40);
  
 
}



void initFrontBumpers()
{
  pinMode(pinFrontBumperLeft,INPUT);
  pinMode(pinFrontBumperRight,INPUT);
}

void initSonars()
{
  //srf02 init
  Wire.begin();             //I2C init (for srf02 sonarsensors)
  SRF02::setInterval(200);  //set srf02 scaninterval to 200 ms
}




void initSerialPort()
{
   delay(500);
   Serial.begin(19200);
   Serial.print("#TMABOT 1.0 mobility firmware initialized;");
   Serial.print("id=");
   Serial.print(DEVICE_ID);
   Serial.println();
  
}

void sendParameters()
{
    if (Serial.available()>2)
    {
      int intByte1=Serial.read();
      int intByte2=Serial.read();
      int intByte3=Serial.read();
    }
    
    
    Serial.print("id="); Serial.print(DEVICE_ID);       Serial.print(';');
    Serial.print("st="); Serial.print(_state);          Serial.print(';');
    Serial.print("ms="); Serial.print(_motorSpeed);     Serial.print(';');
    Serial.print("md="); Serial.print(_motorDirection); Serial.print(';');
    Serial.print("dl="); Serial.print(_distanceLeft);   Serial.print(';');
    Serial.print("dr="); Serial.print(_distanceRight);  Serial.print(';');
    Serial.print("dc="); Serial.print(_distanceCenter); Serial.print(';');
    Serial.print("cd="); Serial.print(_compassAngle);   Serial.print(';');
    Serial.print("#=");  Serial.print(_sequence);
    Serial.println();
    
   
    
}



void updateFrontBumper()
{
  int buttonStateL=digitalRead(pinFrontBumperLeft);
  int buttonStateR=digitalRead(pinFrontBumperRight);
  if (buttonStateL<1 || buttonStateR<1)
  {
    _state=STATE_FRONTBUMPER; 
  }
}






//@name setMotor
//@desc sets the motor speed and direction. Drives the h-bridge (motordriver)
//speed int (0..100)
//direction int (DIRECTION_UNDEFINED | DIRECTION_FORWARD | DIRECTION_BACKWARD | DIRECTION_TURNLEFT | DIRECTION_TURNRIGHT)
void setMotors (int speed, int direction)
{

  int motorMinOffset=0;    //minimum amount to get motors spinning
  int motorMaxOffset=255;  //maximum amount to prevent motordrive from burning (limiter)
  int motorPower=map(speed,0,100,motorMinOffset,motorMaxOffset);
  int motorLeftDirection;
  int motorRightDirection;

  //set motor direction
  switch (direction)
  {
  case DIRECTION_UNDEFINED: //turn around own axis
    motorLeftDirection=MOTOR_FORWARD;
    motorRightDirection=MOTOR_BACKWARD;
    break;
  case DIRECTION_FORWARD:
    motorLeftDirection=MOTOR_FORWARD;
    motorRightDirection=MOTOR_FORWARD;
    break;
  case DIRECTION_BACKWARD:
    motorLeftDirection=MOTOR_BACKWARD;
    motorRightDirection=MOTOR_BACKWARD;
    break;
  case DIRECTION_TURNLEFT:
    motorLeftDirection=MOTOR_BACKWARD;
    motorRightDirection=MOTOR_FORWARD;
    break;
  case DIRECTION_TURNRIGHT:
    motorLeftDirection=MOTOR_FORWARD;
    motorRightDirection=MOTOR_BACKWARD;
    break;
  }
  
  digitalWrite(pinMotorLeftDirection,motorLeftDirection);
  digitalWrite(pinMotorRightDirection,motorRightDirection);
  analogWrite(pinMotorLeftPower, motorPower);
  analogWrite(pinMotorRightPower, motorPower);
  
}

void setLeftMotor(int speed, int direction)
{
  digitalWrite(pinMotorRightDirection,direction);
  analogWrite(pinMotorLeftPower, speed);
}


void setRightMotor(int speed, int direction)
{
  digitalWrite(pinMotorRightDirection,direction);
  analogWrite(pinMotorRightPower, speed);
}


int getSharpDistance()
{

  float volts = analogRead(pinDistanceSensor)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  int distance = round(65*pow(volts, -1.10));          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  return distance;
}


void ShiftOut(int Value, int BitsCount) 
{
  for(int i = BitsCount; i >= 0; i--) {
    digitalWrite(pinCompassClock, LOW);
    if ((Value & 1 << i) == ( 1 << i)) {
      digitalWrite(pinCompassDIO, HIGH);
    }
    else {
      digitalWrite(pinCompassDIO, LOW);
    }
    digitalWrite(pinCompassClock, HIGH);
    delayMicroseconds(1);
  }
}

int ShiftIn(int BitsCount) 
{
  int ShiftIn_result;
    ShiftIn_result = 0;
    pinMode(pinCompassDIO, INPUT);
    for(int i = BitsCount; i >= 0; i--) {
      digitalWrite(pinCompassClock, HIGH);
      delayMicroseconds(1);
      if (digitalRead(pinCompassDIO) == HIGH) {
        ShiftIn_result = (ShiftIn_result << 1) + 1; 
      }
      else {
        ShiftIn_result = (ShiftIn_result << 1) + 0;
      }
      digitalWrite(pinCompassClock, LOW);
      delayMicroseconds(1);
    }
 
  if ((ShiftIn_result & 1 << 11) == 1 << 11) {
    ShiftIn_result = (B11111000 << 8) | ShiftIn_result; 
  }


  return ShiftIn_result;
}

void compassReset() {
  pinMode(pinCompassDIO, OUTPUT);
  digitalWrite(pinCompassEnable, LOW);
  ShiftOut(B0000, 3);
  digitalWrite(pinCompassEnable, HIGH);
}

void compassStartAcquisition() {
  pinMode(pinCompassDIO, OUTPUT);
  digitalWrite(pinCompassEnable, LOW);
  ShiftOut(B1000, 3);
  digitalWrite(pinCompassEnable, HIGH);
}

int compassReadCommand() {
  int result = 0;
  pinMode(pinCompassDIO, OUTPUT);
  digitalWrite(pinCompassEnable, LOW);
  ShiftOut(B1100, 3);
  result = ShiftIn(3);
  return result;
}