Holonomic Robotic Platform
Jump to navigation
Jump to search
Holonomic Robot Examples
A holonomic system is when the number of controllable degrees of freedom equals the total degrees of freedom.
Project Purpose
Developing software, and design hardware using sensor integration, with a view to developing skillsets and knowledge concerning holonomic drive systems. This will allow team members to utilize holonomics in the WAM-V Propulsion, Control, Guidance, and Sensor sections during RobotX
Engineering Project Data
Versions of Robotic Platform
- Current Version Platform
Ortho View
Top Down View
Side View
- v1:Development Platform
Top Down View
Side View
Mechanical
3D Printer Technology
- 3D Printers used to print parts for the Crawler Bot
FDM Printer
Filament Reel
Top of X-Max Printer
Front of X-Max Printer
Robot Body and parts
Robot Omni wheels
Omni Wheels and Positioning
- 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 1305535, J. Grabowiecki, "Vehicle wheel", issued 1919-06-03 A variant of the wheel was patented by Josef F. Blumrich in 1972. US patent 3789947, Josef F. Blumrich, "Omnidirectional wheel", issued 1974-02-05
Drive Motors with Gearboxes
- 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
- Available on Amazon
Absorbers and Springs
Drive Shafts with Universal Joints
Hardware Case for Raspberry PI 4
Electronic/Electrical
Raspberry PI4 4 Gig Ram
Pi Setup
- Raspbian System Image was downloaded from 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.
Raspberry PI Full Function Servo Motor HAT Controller
- 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 Raspberry PI Wiki: Robot Expansion Board.
Battery Power 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.
HDMI Video Adapter Cable
Micro SD 64 Gig Memory Card for PI Operating System, and Hard Drive
Software
Raspbian Operating System
Balena Flash Prom Programmer
Windows Python for Laptop Host Development
Spyder Development Environment
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:
- Editor
- IPython Console
- Variable Explorer
- Profiler
- Debugger
- Help
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
Laptop Code
#!/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")
Robot Code
# -*- 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()
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