View source for Holonomic Robotic Platform

=Holonomic Robot Examples=
[[File:Holonomic Frame with Wheels.gif|120px|thumb|right|See Image file for Source]]
<youtube width="400" height="300" >soudy8fKygk</youtube>
{{clear}}
A holonomic system is when the number of controllable degrees of freedom equals the total degrees of freedom.
{{clear}}
=Engineering Project Data=
===Versions of Robotic Platform===
{{Gallery
|title=v1:Development Platform
| width=100 | height=140
|align=left
|footer=
|File:CrawlerBot1.jpg 
 |alt1= Wheels temporarily mounted on angle brackets
 |Top Down View
|File:CrawlerBot2.jpg
 |alt2= Wheels temporarily mounted on angle brackets
 |Side View
}}

{{clear}}

==Mechanical==
===3D Printer Technology===
{{Gallery
|title=3D Printers used to print parts for the Crawler Bot
| width=100 | height=140
|align=left
|footer=
|File:3DPA1.jpg 
 |alt1=FDM Printer Technology
 |FDM Printer
|File:3DPB1.jpg
 |alt2=Filament Reel stored in box to control moisture content
 |Filament Reel
|File:3DPB2.jpg 
 |alt3= View of top of printer
 |Top of X-Max Printer
|File:3DPB3.jpg 
 |alt4=
 |Front of X-Max Printer
}}
{{clear}}
===Robot Body and parts===
<3d scale="2" width="200" height="200" norotate="true">MainBody.stl</3d>
<3d scale="3" width="200" height="200" norotate="true">HolonomicDriveControlArm.stl</3d>
<3d scale="7" width="200" height="200" norotate="true">Second type of wheel hub.stl</3d>
{{clear}}

===Omni Wheels and Positioning===
[[File:Omni wheel.jpg|120px|thumb|right|Typical Omni-Wheel]]
*Omni-wheels are often used to allow for movement on the horizontal axis on a drivetrain, as well as forward and backward movement.
====History====
{{US patent reference
 | number = 1305535
 | y = 1919
 | m = 06
 | d = 03
 | inventor = J. Grabowiecki
 | title = Vehicle wheel
}}
A variant of the wheel was patented by [[Josef F. Blumrich]] in 1972.
{{US patent reference
 | number = 3789947
 | y = 1974
 | m = 02
 | d = 05
 | inventor = Josef F. Blumrich
 | title = Omnidirectional wheel
}}
{{clear}}

===[https://www.adafruit.com/product/3777 Drive Motors with Gearboxes]===
[[File:DC-electric-motor.jpg|120px|thumb|right|Bidirectional Motors]]
*3-6 VDC, Bidirectional
*Gearbox Motor with a gear ratio of 1:48
*2 x 200mm wires with breadboard-friendly 2.54mm(0.1”) male connectors 
*Maximum torque: 800k.cm
*[https://www.amazon.com/gp/product/B07P9ZSWBG/ Available on Amazon] 
{{clear}}

===[https://www.amazon.com/gp/product/B019XU3C3O/Shock Absorbers and Springs]===
[[File:Shock Absorbers.jpg ‎|120px|thumb|right|Miniture Shock Absorbers]]
{{clear}}

===[https://www.amazon.com/gp/product/B081GVBWFX/ Drive Shafts with Universal Joints]===
[[File:UV Joints.jpg ‎|120px|thumb|right|Miniature Drive Shafts]]
{{clear}}


===[https://www.amazon.com/gp/product/B07VZT8MN9/ref=ppx_yo_dt_b_asin_title_o06_s01?ie=UTF8&psc=1 Hardware Case for Raspberry PI 4]===

==Electronic/Electrical==
===[https://www.amazon.com/gp/product/B07TC2BK1X/ref=ppx_yo_dt_b_asin_title_o06_s02?ie=UTF8&psc=1 Raspberry PI4 4 Gig Ram]===
[[File:PI_4.jpg|120px|thumb|right|Raspberry PI 4]]
====Pi Setup====
#Raspbian System Image was downloaded from [https://www.raspberrypi.org/ RaspberryPi.org] to development laptop.
#Belana Etcher Disk Flasher was downloaded and installed on laptop.
#64 Gigabyte Micro SD Card was placed in San Disk adapter and mounted on laptop.
#Micro SD card was flashed with Raspbian OS, then removed from Laptop and inserted in to PI 4.
#HDMI Video 4K Monitor connected to PI via Mini-HDMI adapter, as well as USB Keyboard and Mouse connected to PI 4.
#PI 4 was powered on, and OS Install and Finalization was initiated.
#PI Wi-Fi connection to Router was verified, with static IP address assigned to PI 4.
#PI 4 initiated software updates via internet connect.
#VNC Server was setup on PI 4 to connect to Laptop via Port:5900.
#VSFTPD was installed on PI 4 to handle file transfers between Laptop and PI 4 using FTP client on Port:21.
#PI was Powered Down, and USB and video cable removed from PI, 
#PI was powered up as a stand alone device.
#VNC Wi-Fi connection was initiated between Laptop and PI successfully.
#Development now takes place across Network VNC connection, with PI as a Standalone Computing Device.
{{clear}}

===[https://www.amazon.com/gp/product/B0721MTJ3P Raspberry PI Full Function Servo Motor HAT Controller]===
[[File:Motor_HAT.jpg|120px|thumb|right|Geekworm HAT]]
*Full function Robot Expansion Board (Support Stepper / Motor / Servo) for Raspberry Pi.
*Stepper motors are great for (semi-)precise control, perfect for many robot and CNC projects.
*HAT supports up to 2 stepper motors. The python library works identically for bi-polar and uni-polar motors
*For a complete in-depth explanation of using this HAT see the [http://www.raspberrypiwiki.com/index.php/Robot_Expansion_Board Raspberry PI Wiki: Robot Expansion Board].
{{clear}}

===[https://www.amazon.com/gp/product/B0811PLY9D/ Battery Power Regulator]===
[[File:Battery Voltage regulator.jpg ‎|120px|thumb|right|Voltage regulator]]
*DC In 5.5-30V, Out 0.5-30V adjustable voltage range, 35W 4A high power, buck-boost converter can be used as ordinary Buck power supply module, charger and LED constant current driver. 
*HD LCD display
**input voltage
**output voltage
**current
**power
*Support for charging function 
*anti-reverse protection
*anti-backflow protection,
*short-circuit protection
*over-current protection
*over-temperature protection*
over-voltage protection. 
{{clear}}

===[https://www.amazon.com/gp/product/B00B2HORKE/ref=ppx_yo_dt_b_asin_title_o06_s02?ie=UTF8&psc=1 HDMI Video Adapter Cable]===
===[https://www.amazon.com/gp/product/B073JYVKNX/ref=ppx_yo_dt_b_asin_title_o06_s00?ie=UTF8&psc=1 Micro SD 64 Gig Memory Card for PI Operating System, and Hard Drive]===

==Software==
===[https://www.raspberrypi.org/downloads/raspbian/ Raspbian Operating System]===
===[https://www.balena.io/etcher/ Balena Flash Prom Programmer]===
[[File:Balena Etcher.png|120px|thumb|right|Interface Window]]
{{clear}}

===[https://sourceforge.net/projects/winpython/ Windows Python for Laptop Host Development]===
[[File:Winpython_title.png|120px|thumb|right|Winpython Logo]]

{{clear}}

===[https://www.spyder-ide.org/ Spyder Development Environment]===
[[File:Spyder_IDE.png|120px|thumb|right|Spyder IDE]]
Spyder is a powerful scientific environment written in Python, for Python, and designed by and for scientists, engineers and data analysts, featuring the following options:
{{Flatlist |
*Editor
*IPython Console
*Variable Explorer
*Profiler
*Debugger
*Help
}}
{{clear}}

==Laptop and PI via VNC(Virtual Network Connection)==
To help easy development of specific modules in the Holonomic Robot, a communications interface is implemented between the Robot(Raspberry PI 4 Server) and a Laptop Development workstation(HP Client).  The protocol is built on TCP Tx-Rx Exchange between the two devices. The code modules are Python running at both ends.  The Robot starts out as a purely slaved device taking commands from the laptop.  Motor control modules manipulating the GPIO interface run on the Robot but are commanded by the laptop.  As the motor control is refined and routinized along with the growth of intrinsic autonomy, the code is transitioned from the Laptop to the Robot. Code is primarily developed in Python 3.7.
===PI Window with Thony Python IDE===

===Laptop Python GUI Interface===

==Code and Mathematics==
===Code to Facilitate Development===

====Laptop Code for Robot/Laptop Base Communication ====
<source lang="Python">
#!/usr/bin/python3


import queue
import threading
import time
import itertools
import PySimpleGUI as sg
import socket
    
def worker_thread1(thread_name, gui_queue):
    """
    A worker thread that communicates with the GUI
    These threads can call functions that block without affecting the GUI (a good thing)
    Note that this function is the code started as each thread.
    This thread recieves data from the BOT.
    """
    while True:
        message_from_bot = (client.recv(4096)).decode()
        gui_queue.put(message_from_bot)
        #print (message_from_bot)
        #if message_from_bot == "RxAck:Engaging Robot Shutdown" : escape = False 
        
def worker_thread2(thread_name, run_freq,  gui_queue):
        gui_queue.put('{} - {}'.format(thread_name))
        
def worker_thread3(thread_name, run_freq,  gui_queue):
        gui_queue.put('{} - {}'.format(thread_name))
        
def the_gui(gui_queue):
    GRAPH_SIZE = (400,400)
    GRAPH_STEP_SIZE = 1
    sg.change_look_and_feel('GreenMono') # give our window a spiffy set of colors
    # ------ GetFile Window Definition ------ #
    
    # ------ Menu Definition ------ #
    menu_def = [['&File', ['&Open Text File to Transmit', '---', 'Properties', '---','H&alt Everything']],
                ['&Help', '&About...'], ]
    # ------ Column Definition ------ #
    col1 = [    
                [sg.Text('Bot Message response', size=(20, 1))],
                [sg.Output(size=(30, 20), font=('Mono 8'))],
                [sg.Text('Bot Command Single Entry', size=(20, 1))],
                [sg.InputText(size=(30, 0), key='-QUERY-')],
                [sg.Button('Send Command',size=(20,1), button_color=(sg.YELLOWS[0], sg.GREENS[0]), bind_return_key=True)],
                ]    
    col2 = [    
                [sg.Text('Bot Jitter Test',font=('Mono 9'), justification="center", size=(48, 1))],
                [sg.Button('<<CCW',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('S2S-F2B',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('Combo',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('CW>>',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0]))],
                
                [sg.Text('Bot Rotation',font=('Mono 9'), justification="center", size=(48, 1))],
                [sg.Button('<<180 CCW',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('<180 CCW',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('180 CW>',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('180 CW>>',size=(10,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),],
                
                [sg.Button('<<Input CCW',size=(15,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.InputText(size=(11, 0), justification="center", key='-Rotate-'),
                sg.Button('Input CW>>',size=(15,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                ],
                 
                [sg.Text('IMU',font=('Mono 9'), justification="center", size=(48, 1))],
                [sg.Button('Zero to North',size=(16,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])), 
                sg.Button('South',size=(8,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])), 
                sg.Button('East',size=(8,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),  
                sg.Button('West',size=(8,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),],  
                 
                [sg.Text('Linear Movement',font=('Mono 9'), justification="center", size=(48, 1))],
                [sg.Text('Bot Angle:',font=('Mono 9'), justification="Left", size=(10, 1)),
                sg.InputText(size=(5, 0), justification="center", key='-BotAngle-'),
                sg.Text('Line of Bearing:',font=('Mono 9'), justification="Left", size=(15, 1)),
                sg.InputText(size=(5, 0), justification="center", key='-Bearing-'),],
                [sg.Text('Speed:',font=('Mono 9'), justification="Left", size=(8, 1)),
                sg.Slider(range=(1,255),enable_events=False, key='-velocity-', default_value=128, size=(22,15), orientation='horizontal', font=('Helvetica', 12)),],
                [sg.Button('Execute Move',size=(15,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),
                sg.Button('Execute Move with Spin',size=(30,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0])),],
                ]    
    col3 = [
                [sg.Graph(GRAPH_SIZE, (10,10), GRAPH_SIZE, key='-GRAPH-', background_color='lightblue')],
                ]
    col4 = [
                [sg.Checkbox('Auto Data RecX',font=('Mono 9'), size=(15, 1), default=False, enable_events=True, key='-ADRecX-')],
                [sg.Button('Gyroscope',size=(16,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0]))],
                [sg.Text('x',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Gx-'),
                sg.Text('y',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Gy-'),
                sg.Text('z',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Gz-'),],
                [sg.Button('Acceleration',size=(15,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0]))],
                [sg.Text('x',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Ax-'),
                sg.Text('y',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Ay-'),
                sg.Text('z',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Az-'),],
                [sg.Button('Magnetic',size=(15,1),font=('Mono 9'), button_color=(sg.YELLOWS[0], sg.GREENS[0]))],
                [sg.Text('x',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Mx-'),
                sg.Text('y',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-My-'),
                sg.Text('z',size=(1, 1),pad=(0,0)),sg.Text(size=(4, 1),font=('Mono 8'), key='-Mz-'),],
                ]            
    layout = [
          [sg.Menu(menu_def, tearoff=True)],
          [sg.Column(col1, pad=((1,1),(1,1)), justification="left", background_color = None),
           sg.Column(col2, pad=((1,1),(1,1)), justification="left", background_color = None),
           sg.Column(col3, pad=((1,1),(1,1)), justification="left", background_color = None),
           sg.Column(col4, pad=((1,1),(1,1)), justification="left", background_color = None),
          ]]

    
    main_window = sg.Window('Crawler Bot Window Control', layout, font=('Helvetica', ' 11'), default_button_element_size=(8, 2))
    # --------------------- EVENT LOOP ---------------------
    escape = True
    message_to_bot = ''
    while True and escape == True:
        # wait for up to 100 ms for a GUI event
        event, values = main_window.read(timeout=100)
        if event in (None, 'Exit'):
            break
        if event == 'Open Text File to Transmit':
            text = sg.PopupGetFile("Open the file to transmit",
            save_as=False,
            file_types=(('ALL Files', '*.*'),),
            no_window=False,
            location=(None, None),
            initial_folder=None)
             #transmit line by line to the bot
            print ("Transmitting Message")
            client.send(message_to_bot.encode())
        elif event == 'Halt Everything':
            message_to_bot = "SRS"
            client.send(message_to_bot.encode())
            break
        elif event == 'Gyroscope':
            message_to_bot = "GYR"
            client.send(message_to_bot.encode())
        elif event == 'Acceleration':
            message_to_bot = "ACC"
            client.send(message_to_bot.encode())
        elif event == 'Magnetic':
            message_to_bot = "MAG"
            client.send(message_to_bot.encode())
        elif event == '-velocity-':
            #this reads the speed, then sends the speed message
            print("Speed Adjustment")
        elif event == '<<Input CCW':
            #this reads the number of degrees to rotate and gets the speed, then sends the messages
            message_to_bot = "SPD" + str(int((values['-velocity-']))).rstrip()
            client.send(message_to_bot.encode()) 
            message_to_bot = "ROT" + values['-Rotate-'].rstrip()
            client.send(message_to_bot.encode()) 
        elif event == 'Input CW>>':
            #this reads the number of degrees to rotate and gets the speed, then sends the messages
            message_to_bot = "SPD" + str(int((values['-velocity-']))).rstrip()
            client.send(message_to_bot.encode()) 
            message_to_bot = "ROT-" + values['-Rotate-'].rstrip()
            client.send(message_to_bot.encode()) 
        elif event == '<<CCW':
            #this code does the jitter test in the CCW direction
            message_to_bot = "SPD" + str(int((values['-velocity-']))).rstrip()
            client.send(message_to_bot.encode())            
            message_to_bot = "TST1"
            client.send(message_to_bot.encode())
        elif event == 'CW>>':
            #this code does the jitter test in the CW direction
            message_to_bot = "SPD" + str(int((values['-velocity-']))).rstrip()
            client.send(message_to_bot.encode())            
            message_to_bot = "TST2"
            client.send(message_to_bot.encode())           
        elif event == 'S2S-F2B':
            #this code does the jitter test in the F/R S/S directions
            message_to_bot = "SPD" + str(int((values['-velocity-']))).rstrip()
            client.send(message_to_bot.encode())            
            message_to_bot = "TST3"
            client.send(message_to_bot.encode())            
            
        elif event == 'Send Command':
            message_to_bot = values['-QUERY-'].rstrip()
            client.send(message_to_bot.encode())
        elif event == 'Auto Data RecX':
            print("AD")
            #client.send(message_to_bot.encode())            
        # --------------- Loop through all messages coming in from threads ---------------
        while True:                 # loop executes until runs out of messages in Queue
            try:                    # see if something has been posted to Queue
                message = gui_queue.get_nowait()
            except queue.Empty:     # get_nowait() will get exception when Queue is empty
                break               # break from the loop if no more messages are queued up
            # if message received from queue, display the message in the Window
            if message:
                #main_window['-QUERY-'].update(message)
                # do a refresh because could be showing multiple messages before next Read
                main_window.refresh()
                if message[:3] == "Sys":
                        message = message[4:]
                        if message[:2] == "G:":
                            message = message[3:]
                            xyz = message.split(',')
                            main_window['-Gx-'].update(xyz[0])
                            main_window['-Gy-'].update(xyz[1])
                            main_window['-Gz-'].update(xyz[2])
                        elif message[:2] == "A:":
                            message = message[3:]
                            xyz = message.split(',')
                            main_window['-Ax-'].update(xyz[0])
                            main_window['-Ay-'].update(xyz[1])
                            main_window['-Az-'].update(xyz[2])
                        elif message[:2] == "M:":
                            message = message[3:]
                            xyz = message.split(',')
                            main_window['-Mx-'].update(xyz[0])
                            main_window['-My-'].update(xyz[1])
                            main_window['-Mz-'].update(xyz[2])
                        else:
                            print("-")
                else:            
                    print(message)                
                if message == "RxAck:Engaging Robot Shutdown" : escape = False
    # if user exits the window, then close the window and exit the GUI func
    main_window.close()

if __name__ == '__main__':
    client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client.connect(('192.168.1.132', 13000))
    # -- Create a Queue to communicate with GUI --
    # queue used to communicate between the gui and the threads
    gui_queue = queue.Queue()
    # -- Start threads, each taking a different amount of time
    threading.Thread(target=worker_thread1, args=(
        'Thread 1', gui_queue,),  daemon=True).start()
    #threading.Thread(target=worker_thread2, args=(
    #    'Thread 2', 200, gui_queue,),  daemon=True).start()
    #threading.Thread(target=worker_thread3, args=(
    #    'Thread 3', 1000, gui_queue,),  daemon=True).start()
    # -- Start the GUI passing in the Queue --
    the_gui(gui_queue)

    client.close()
    print ("You ended the Robot Server Session")

  
</source>

====Robot Code for Robot/Laptop Base Communication ====
<source lang="Python">
# -*- coding: utf-8 -*-
#author: joel.martin
#import the modules required here

import queue
import threading
import os
import socket
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor
import time
import atexit
import MPU9250
import sys
#Global Variable Declaration
global_speed = 128
global_gyro_x = 0.0
global_gyro_y = 0.0
global_gyro_z = 0.0
global_accel_x = 0.0
global_accel_y = 0.0
global_accel_z = 0.0
global_mag_x = 0.0
global_mag_y = 0.0
global_mag_z = 0.0
#---------------------------------------------------------
def worker_thread1(thread_name, action_queue):
    """
    A worker thread that communicates with the GUI
    These threads can call functions that block without affecting the GUI (a good thing)
    Note that this function is the code started as each thread.
    This thread recieves data from the BOT.
    """
    while True:
        data = (conn.recv(4096)).decode()
        #if not data: break
        action_queue.put(data)
        #print (message_from_bot)
        #if message_from_bot == "RxAck:Engaging Robot Shutdown" : escape = False 

def worker_thread2(thread_name,  action_queue):
    global global_gyro_x, global_gyro_y, global_gyro_z
    global global_accel_x, global_accel_y, global_accel_z
    global global_mag_x, global_mag_y, global_mag_z
    count = 0
    nCounter = 0
    gyro_x = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    gyro_y = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    gyro_z = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    accel_x = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    accel_y = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    accel_z = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]    
    mag_x = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    mag_y = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
    mag_z = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]  
    #gui_queue.put('{} - {}'.format(thread_name))
    while True:
        count = count + 1
        nCounter = nCounter + 1
        if count == 10: count = 0
        gyro = mpu9250.readGyro()
        gyro_x[count] = gyro['x']
        gyro_y[count] = gyro['y']
        gyro_z[count] = gyro['z']
        global_gyro_x = sum(gyro_x)/10
        global_gyro_y = sum(gyro_y)/10
        global_gyro_z = sum(gyro_z)/10
        accel = mpu9250.readAccel()
        accel_x[count] = accel['x']
        accel_y[count] = accel['y']
        accel_z[count] = accel['z']
        global_accel_x = sum(accel_x)/10
        global_accel_y = sum(accel_y)/10
        global_accel_z = sum(accel_z)/10
        accel = mpu9250.readMagnet()
        
        
        #time.sleep(.02)
        if (nCounter > 99):
            nCounter = 0
            #grab gyro data
            message = ("Sys:G:" + str(global_gyro_x) +"," +str(global_gyro_y) +"," +str(global_gyro_z))
            conn.send(message.encode())
        if (nCounter == 66):
            message = ("Sys:A:" + str(global_accel_x) +"," +str(global_accel_y) +"," +str(global_accel_z))
            conn.send(message.encode())
       # if (nCounter == 3):   
        #    message = ("Sys:M:" + str(mag['x']) +"," +str(mag['y']) +"," +str(mag['z']))
         #   conn.send(message.encode())         
        
        
        
def worker_thread3(thread_name, run_freq,  action_queue):
        gui_queue.put('{} - {}'.format(thread_name))
        
        
def action(action_queue):
    escape = True
    message = ''
    nCounter=0
    gFlag=True    
    while True and escape == True:
        message_from_client = ""
        nCounter = nCounter + 1
        #cyclical telemetry messages back to laptop control go here

        if (nCounter == 33333 and gFlag == True):
            mag = mpu9250.readMagnet()

         # --------------- Loop through all messages coming in from threads ---------------
        while True:                 # loop executes until runs out of messages in Queue
            try:                    # see if something has been posted to Queue
                message_from_client = action_queue.get_nowait()
            except queue.Empty:     # get_nowait() will get exception when Queue is empty
                break               # break from the loop if no more messages are queued up
            # if message received from queue, display the message in the Window            
            if message_from_client:
                if message_from_client[:3] == "SRS":
                    print ('Coms and Robot Control termintated at Server by Client')
                    #robot server system shutdown, terminate everything
                    turnOffMotors()
                    escape = False
                elif message_from_client[:3] == "ACC":
                    accel = mpu9250.readAccel()
                    message = ("Sys:A:" + str(accel['x']) +"," +str(accel['y']) +"," +str(accel['z']))
                    conn.send(message.encode()) 
                elif message_from_client[:3] == "GYR":
                    gyro = mpu9250.readGyro()
                    message = ("Sys:G:" + str(global_gyro_x) +"," +str(gyro['y']) +"," +str(gyro['z']))
                    conn.send(message.encode())                
                elif message_from_client[:3] == "MAG":
                    mag = mpu9250.readMagnet()
                    message = ("Sys:M:" + str(mag['x']) +"," +str(mag['y']) +"," +str(mag['z']))
                    conn.send(message.encode()) 
                elif message_from_client[:3] == "MOT":  
                    message = "RxAck:Engaging Motor Message"
                    conn.send(message.encode())
                elif message_from_client[:3] == "SRS":
                    turnOffMotors()
                    message = "RxAck:Engaging Robot Shutdown"
                    conn.send(message.encode())
                    escape = False
                elif message_from_client[:3] == "TST":
                    message = "RxAck;Testing Motors"
                    conn.send(message.encode())
                    message = "RxMsg;Testing 1\n"
                    conn.send(message.encode())
                    message = "RxMsg;Testing 2\n"
                    conn.send(message.encode())
                    message = "RxMsg;Testing 3\n"
                    conn.send(message.encode())
                    message = "RxMsg:Testing 4\n"
                    conn.send(message.encode())
                    motortest(int(message_from_client[3:]))
                elif message_from_client[:3] == "ROT":
                    message = "RxAck;Rotating bot"
                    conn.send(message.encode())
                    rotate_bot(int(message_from_client[3:]),global_speed)
                    message = "RxAck:Rotated"
                    conn.send(message.encode()) 
                elif message_from_client[:3] == "SPD":
                    print (message_from_client[3:])
                    global_speed = int(message_from_client[3:])
                elif message_from_client[:3] == "ADT":
                    conn.send(("RxAck: ADT").encode())
                    gFlag = True
                elif message_from_client[:3] == "ADF":
                    conn.send(("RxAck: ADF").encode())
                    gFlag = False
                else:
                    message = "RxAck:Huh?" + message_from_client
                    conn.send(message.encode())

def turnOffMotors():
    mh.getMotor(1).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(2).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(3).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(4).run(Raspi_MotorHAT.RELEASE)

def motorcontrol(motor,speed,direction):
    if (speed == 0):
        mh.getMotor(motor).run(Raspi_MotorHAT.RELEASE)
    else:
        mh.getMotor(motor).setSpeed(speed) 
        if (direction == "FOR"):
            mh.getMotor(motor).run(Raspi_MotorHAT.FORWARD)
        elif (direction == "REV"):
            mh.getMotor(motor).run(Raspi_MotorHAT.BACKWARD)
        elif (direction == "REL"):
            mh.getMotor(motor).run(Raspi_MotorHAT.RELEASE)
            
def motortest(i):
    if i == 1:
        motorcontrol(1,128,"REV")
        motorcontrol(2,128,"REV")
        motorcontrol(3,128,"REV")
        motorcontrol(4,128,"REV")
        time.sleep(.1)
        motorcontrol(1,128,"FOR")
        motorcontrol(2,128,"FOR")
        motorcontrol(3,128,"FOR")
        motorcontrol(4,128,"FOR")
        time.sleep(.1)
        motorcontrol(1,128,"REL")
        motorcontrol(2,128,"REL")
        motorcontrol(3,128,"REL")
        motorcontrol(4,128,"REL")
    if i == 2:
        motorcontrol(1,128,"FOR")
        motorcontrol(2,128,"FOR")
        motorcontrol(3,128,"FOR")
        motorcontrol(4,128,"FOR")
        time.sleep(.1)
        motorcontrol(1,128,"REV")
        motorcontrol(2,128,"REV")
        motorcontrol(3,128,"REV")
        motorcontrol(4,128,"REV")
        time.sleep(.1)
        motorcontrol(1,128,"REL")
        motorcontrol(2,128,"REL")
        motorcontrol(3,128,"REL")
        motorcontrol(4,128,"REL")    
    if i == 3:
        motorcontrol(1,128,"FOR")
        motorcontrol(3,128,"REV")
        time.sleep(.1)       
        motorcontrol(1,128,"REV")
        motorcontrol(3,128,"FOR")
        time.sleep(.2)          
        motorcontrol(1,128,"FOR")
        motorcontrol(3,128,"REV")
        time.sleep(.1)
        motorcontrol(1,128,"REL")
        motorcontrol(3,128,"REL")
        #alt axis
        motorcontrol(2,128,"FOR")
        motorcontrol(4,128,"REV")
        time.sleep(.1)       
        motorcontrol(2,128,"REV")
        motorcontrol(4,128,"FOR")
        time.sleep(.2)          
        motorcontrol(2,128,"FOR")
        motorcontrol(4,128,"REV")
        time.sleep(.1)                
        #all off
        motorcontrol(2,128,"REL")
        motorcontrol(4,128,"REL")         
def rotate_bot(direction,speed):
        print (direction)
        if (direction > 0):
            motorcontrol(1,speed,"REV")
            motorcontrol(2,speed,"REV")
            motorcontrol(3,speed,"REV")
            motorcontrol(4,speed,"REV")
        elif (direction < 0):
            motorcontrol(1,speed,"FOR")
            motorcontrol(2,speed,"FOR")
            motorcontrol(3,speed,"FOR")
            motorcontrol(4,speed,"FOR")        
        time.sleep (.023*abs(direction))
        #look at the IMU and loop until reaching degree limit
        turnOffMotors()
        
if __name__ == '__main__':
    # create a default object, no changes to I2C address or frequency
    mh = Raspi_MotorHAT(addr=0x6f)
    mpu9250 = MPU9250.MPU9250()
    #setup the communications with the development control here
    serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    serv.bind(('192.168.1.132',13000)) #this is for the Rasberry pi
    serv.listen(5)
    print ("Going into commanded mode")
    #escape = True
    #while True:
    conn, addr = serv.accept()
    message_from_client = ''
    action_queue = queue.Queue()
    # -- Start thread 1
    threading.Thread(target=worker_thread1, args=(
        'Thread 1', action_queue,),  daemon=True).start()
    # -- Start thread 2
    threading.Thread(target=worker_thread2, args=(
        'Thread 2', action_queue,),  daemon=True).start()
            #threading.Thread(target=worker_thread3, args=(
            #    'Thread 3', 1000, action_queue,),  daemon=True).start()
    # -- Start the acton Subroutine passing in the Queue --
    action(action_queue)
    
    conn.close() 
    

    

</source>

===Mathematics===
See this journal article:
Rojas, R., & Forster, A. (2006). Holonomic Control of a robot with an omnidirectional drive. Kunstliche Intelligenz, BottcherIT Verlag. Retrieved from https://www.mindraces.org/public_references/idsia_publications/omnidrive_kiart_preprint.pdf