Changes

Jump to navigation Jump to search

Holonomic Robotic Platform (view source)

Revision as of 01:51, 4 May 2020

3,235 bytes added ,  01:51, 4 May 2020
m
→‎Robot Code for Robot/Laptop Base Communication
Line 347: Line 347:  
import MPU9250
 
import MPU9250
 
import sys
 
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):
 
def worker_thread1(thread_name, action_queue):
Line 363: Line 373:  
         #if message_from_bot == "RxAck:Engaging Robot Shutdown" : escape = False  
 
         #if message_from_bot == "RxAck:Engaging Robot Shutdown" : escape = False  
   −
def worker_thread2(thread_name, run_freq,  action_queue):
+
def worker_thread2(thread_name,  action_queue):
−
        gui_queue.put('{} - {}'.format(thread_name))
+
    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):
 
def worker_thread3(thread_name, run_freq,  action_queue):
Line 374: Line 433:  
     message = ''
 
     message = ''
 
     nCounter=0
 
     nCounter=0
−
     gFlag=False    
+
     gFlag=True    
 
     while True and escape == True:
 
     while True and escape == True:
 
         message_from_client = ""
 
         message_from_client = ""
 
         nCounter = nCounter + 1
 
         nCounter = nCounter + 1
 
         #cyclical telemetry messages back to laptop control go here
 
         #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):
 
         if (nCounter == 33333 and gFlag == True):
 
             mag = mpu9250.readMagnet()
 
             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 ---------------
 
         # --------------- Loop through all messages coming in from threads ---------------
 
         while True:                # loop executes until runs out of messages in Queue
 
         while True:                # loop executes until runs out of messages in Queue
Line 414: Line 461:  
                 elif message_from_client[:3] == "GYR":
 
                 elif message_from_client[:3] == "GYR":
 
                     gyro = mpu9250.readGyro()
 
                     gyro = mpu9250.readGyro()
−
                     message = ("Sys:G:" + str(gyro['x']) +"," +str(gyro['y']) +"," +str(gyro['z']))
+
                     message = ("Sys:G:" + str(global_gyro_x) +"," +str(gyro['y']) +"," +str(gyro['z']))
 
                     conn.send(message.encode())                 
 
                     conn.send(message.encode())                 
 
                 elif message_from_client[:3] == "MAG":
 
                 elif message_from_client[:3] == "MAG":
Line 433: Line 480:  
                     message = "RxMsg;Testing 1\n"
 
                     message = "RxMsg;Testing 1\n"
 
                     conn.send(message.encode())
 
                     conn.send(message.encode())
−
                    motortest(1)
   
                     message = "RxMsg;Testing 2\n"
 
                     message = "RxMsg;Testing 2\n"
 
                     conn.send(message.encode())
 
                     conn.send(message.encode())
−
                    motortest(2)
   
                     message = "RxMsg;Testing 3\n"
 
                     message = "RxMsg;Testing 3\n"
 
                     conn.send(message.encode())
 
                     conn.send(message.encode())
−
                    motortest(3)
   
                     message = "RxMsg:Testing 4\n"
 
                     message = "RxMsg:Testing 4\n"
 
                     conn.send(message.encode())
 
                     conn.send(message.encode())
−
                     motortest(4)
+
                     motortest(int(message_from_client[3:]))
 
                 elif message_from_client[:3] == "ROT":
 
                 elif message_from_client[:3] == "ROT":
−
                     message = "RxAck;Rotating Robot"
+
                     message = "RxAck;Rotating bot"
−
                     conn.send(message.encode())              
+
                     conn.send(message.encode())
−
                     rotate_bot(30,255)
+
                     rotate_bot(int(message_from_client[3:]),global_speed)
 
                     message = "RxAck:Rotated"
 
                     message = "RxAck:Rotated"
 
                     conn.send(message.encode())  
 
                     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":
 
                 elif message_from_client[:3] == "ADT":
 
                     conn.send(("RxAck: ADT").encode())
 
                     conn.send(("RxAck: ADT").encode())
Line 478: Line 525:  
              
 
              
 
def motortest(i):
 
def motortest(i):
−
     motorcontrol(i,100,"FOR")
+
     if i == 1:
−
     time.sleep(.1)
+
        motorcontrol(1,128,"REV")
−
    motorcontrol(i,100,"REV")
+
        motorcontrol(2,128,"REV")
−
    time.sleep(.1)
+
        motorcontrol(3,128,"REV")
−
    motorcontrol(i,100,"REL")                 
+
        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):
 
def rotate_bot(direction,speed):
 +
        print (direction)
 
         if (direction > 0):
 
         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(1,speed,"FOR")
 
             motorcontrol(2,speed,"FOR")
 
             motorcontrol(2,speed,"FOR")
 
             motorcontrol(3,speed,"FOR")
 
             motorcontrol(3,speed,"FOR")
−
             motorcontrol(4,speed,"FOR")
+
             motorcontrol(4,speed,"FOR")         
−
        elif (direction < 0):
+
         time.sleep (.023*abs(direction))
−
            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
 
         #look at the IMU and loop until reaching degree limit
 
         turnOffMotors()
 
         turnOffMotors()
Line 513: Line 610:  
     message_from_client = ''
 
     message_from_client = ''
 
     action_queue = queue.Queue()
 
     action_queue = queue.Queue()
−
            # -- Start threads, each taking a different amount of time
+
    # -- Start thread 1
 
     threading.Thread(target=worker_thread1, args=(
 
     threading.Thread(target=worker_thread1, args=(
 
         'Thread 1', action_queue,),  daemon=True).start()
 
         'Thread 1', action_queue,),  daemon=True).start()
−
            #threading.Thread(target=worker_thread2, args=(
+
    # -- Start thread 2
−
            #    'Thread 2', 200, action_queue,),  daemon=True).start()
+
    threading.Thread(target=worker_thread2, args=(
 +
        'Thread 2', action_queue,),  daemon=True).start()
 
             #threading.Thread(target=worker_thread3, args=(
 
             #threading.Thread(target=worker_thread3, args=(
 
             #    'Thread 3', 1000, action_queue,),  daemon=True).start()
 
             #    'Thread 3', 1000, action_queue,),  daemon=True).start()
Line 524: Line 622:  
      
 
      
 
     conn.close()  
 
     conn.close()  
 +
   
 +
 
      
 
      
  
Joelmartin
Bots, Bureaucrats, contributor, Interface administrators, lookupuser, Administrators
4,000

edits

Retrieved from "http://OCRobotx.org/mediawiki/index.php/Special:MobileDiff/13218"

Navigation menu