4,000
edits
Changes
Jump to navigation
Jump to search
mLine 334:
Line 334: + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
→Robot Code for Robot/Laptop Base Communication
====Robot Code for Robot/Laptop Base Communication ====
====Robot Code for Robot/Laptop Base Communication ====
<source lang="Python">
<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
#---------------------------------------------------------
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, run_freq, action_queue):
gui_queue.put('{} - {}'.format(thread_name))
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=False
while True and escape == True:
message_from_client = ""
nCounter = nCounter + 1
#cyclical telemetry messages back to laptop control go here
if (nCounter > 99999 and gFlag == True):
nCounter = 0
#grab gyro data
gyro = mpu9250.readGyro()
message = ("Sys:G:" + str(gyro['x']) +"," +str(gyro['y']) +"," +str(gyro['z']))
conn.send(message.encode())
#grab accel data
if (nCounter == 66666 and gFlag == True):
accel = mpu9250.readAccel()
message = ("Sys:A:" + str(accel['x']) +"," +str(accel['y']) +"," +str(accel['z']))
conn.send(message.encode())
#grab magnetic data
if (nCounter == 33333 and gFlag == True):
mag = mpu9250.readMagnet()
message = ("Sys:M:" + str(mag['x']) +"," +str(mag['y']) +"," +str(mag['z']))
conn.send(message.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_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(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())
motortest(1)
message = "RxMsg;Testing 2\n"
conn.send(message.encode())
motortest(2)
message = "RxMsg;Testing 3\n"
conn.send(message.encode())
motortest(3)
message = "RxMsg:Testing 4\n"
conn.send(message.encode())
motortest(4)
elif message_from_client[:3] == "ROT":
message = "RxAck;Rotating Robot"
conn.send(message.encode())
rotate_bot(30,255)
message = "RxAck:Rotated"
conn.send(message.encode())
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):
motorcontrol(i,100,"FOR")
time.sleep(.1)
motorcontrol(i,100,"REV")
time.sleep(.1)
motorcontrol(i,100,"REL")
def rotate_bot(direction,speed):
if (direction > 0):
motorcontrol(1,speed,"FOR")
motorcontrol(2,speed,"FOR")
motorcontrol(3,speed,"FOR")
motorcontrol(4,speed,"FOR")
elif (direction < 0):
motorcontrol(1,speed,"REV")
motorcontrol(2,speed,"REV")
motorcontrol(3,speed,"REV")
motorcontrol(4,speed,"REV")
time.sleep (3)
#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 threads, each taking a different amount of time
threading.Thread(target=worker_thread1, args=(
'Thread 1', action_queue,), daemon=True).start()
#threading.Thread(target=worker_thread2, args=(
# 'Thread 2', 200, 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>
</source>