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.

   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. 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.

   #include "tatrabot.h"
   #include "demo.h"
   int main(void) {
     tatrabotInit();
     //robotNavigate();
   }

   void robotNavigate(void) {
     int state = 0;
     int currentSpeed = 7000;
     int motorState = 0;
     int robotLeft = false;
     motorsOff();
     int speedL, speedR;
     speedL = speedR = currentSpeed;
     countA = countB = 0;
     while (!senseButton(4))
       ;
     chThdSleepMilliseconds(100);
     while (!senseButton(3)) {
       uint8_t chr = 'x';
       sdAsynchronousRead(&SD3, &chr, 1);
       switch (chr) {
         case 's':
           state = 1;
           countA = countB = 0;
           break;
         case '!':
           state = 0;
           break;
         default:
           chr = 'x';
       }
       if ((distance > 1300) && (state == 1)) {
         chSequentialStreamPut(&SD3, '!');
         state = 2;
       }
       switch (state) {
         case 0:
           motorsOff();
           break;
         case 1:
           switch (motorState) {
             case 0:
               if (countA > countB + 1) {
                 speedR -= 2000;
                 motorState = 1;
               }
               else if (countA < countB - 1) {
                 speedL -= 2000;
                 motorState = 2;
               }
               break;
             case 1:
               if (countA < countB) {
                 speedR = currentSpeed;
                 motorState = 0;
               }
               break;
             case 2:
               if (countA > countB) {
                 speedL = currentSpeed;
                 motorState = 0;
               }
               break;
           }
           setMotor(1, speedL);
           setMotor(2, speedR);
           break;
         case 2:
           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;
       }
     }
   }
   void turnLeft(int speed, int time) {
     setMotor(1, speed);
     setMotor(2, -speed);
     chThdSleepMilliseconds(time);
   }
   void turnRight(int speed, int time) {
     setMotor(1, -speed);
     setMotor(2, speed);
     chThdSleepMilliseconds(time);
   }

Záver

In the end the robots didn't behave as I anticipated, because of the precision of the rotation sensors.