Tatrabot coordination

Project objective

The goal of my project was to create a program for a pair of TATRABOT robots, such they will be capable to navigate synchronously, while carrying a pole. Another task was to make it, so that the whichever robot detects a obstruction, they will try to navigate around it while trying to not drop the pole.

Hardware

For this project I have used a pair of TATRABOTs. Each bot has a "saddle" that carries the rod and can rotate freely.

Software

My project has two components. One is the program that runs on the robot. The another one is a relay, that handles the messaging between the bots, because the bluetooth chip cannot be set to Master mode to connect to another device. So that's why there is a need of a relay.

I have written this in Python 3.5, because of the ease of use of the language. The program uses two threads where opens the serial bluetooth connections given by the user, and relays the incoming messages from one thread to another. For the serial connection I have used the pySerial python library, so to compile the relay code you have to download, and install it from pip or [1].

   from queue import Queue
   from threading import Thread
   from serial import *
   import os
   class Relay(object):
       def __init__(self, ports):
           self.serials = [Serial(port=p, baudrate="57600") for p in ports]
           self.threads = []
           self.alive = False
       def start(self):
           self.alive = True
           self.threads.append(Thread(target=self.transmit,args=(self.serials[0],self.serials[1])))
           self.threads.append(Thread(target=self.transmit, args=(self.serials[1], self.serials[0])))
           for thread in self.threads:
               thread.daemon = True
               thread.start()
       def stop(self):
           self.alive = False
       def join(self):
           for thread in self.threads:
               while thread.isAlive():
                   thread.join(0.1)
       def transmit(self,source, target):
           source.write(bytes([115]))
           while self.alive:
               read = source.read()
               if ord(read) == 112:
                   data = source.read()
                   print("{0}-Print: {1}".format(str(source.port), str(chr(ord(data)))))
               else:
                   if target:
                       target.write(read)
                       print("{0}-Message: {1}".format(str(source.port), str(read)))
                   else:
                       print("{0}-Message: {1}".format(str(source.port), str(read)))
   if __name__ == "__main__":
       relay = Relay(["COM14","COM17"])
       relay.start()
       relay.join()

Program for the robot. To write and compile the program for the Tatrabots I have used ChibiStudio

The variable "robotLeft" is used to set on which side is the robot on. (true=left, false=right) First Start the robots, and then press the 4th button. Now you we can start the relay what send the start message to each robot. The start message is just an 's' character sent with this command.chSequentialStreamPut(&SD3, 's'); The code to go around the obstacle.

           setMotor(3, -currentSpeed);
           chThdSleepMilliseconds(1500);
           if (!robotLeft)
             turnLeft(4000, 700);
           else
             turnRight(4000, 700);
           setMotor(3, -currentSpeed);
           chThdSleepMilliseconds(2000);
           if (!robotLeft)
             turnRight(4000, 700);
           else
             turnLeft(4000, 700);
           chSequentialStreamPut(&SD3, 's');
           setMotor(3, currentSpeed);
           chThdSleepMilliseconds(4000);
           chSequentialStreamPut(&SD3, '!');
           if (!robotLeft)
             turnLeft(4000, 700);
           else
             turnRight(4000, 700);
           setMotor(3, currentSpeed);
           chThdSleepMilliseconds(2000);
           if (!robotLeft)
             turnRight(4000, 700);
           else
             turnLeft(4000, 700);
           setMotor(3, currentSpeed);
           chThdSleepMilliseconds(1500);
           state = 1;
           chSequentialStreamPut(&SD3, 's');
           break;

Conclusion

In the end I have failed to complete the objective fully. The robots didn't behave as I anticipated, because of the precision of the rotation sensors. Maybe the rod was too heavy for the motors.