Changes

Jump to navigation Jump to search

Holonomic Robotic Platform (view source)

Revision as of 04:18, 7 February 2020

3,482 bytes added ,  04:18, 7 February 2020
m
→‎Robot Code for Robot/Laptop Base Communication
Line 175: Line 175:  
====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 os
 
import socket
 
import socket
−
serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor
−
serv.bind(('192.168.1.43',13000))
+
import time
−
serv.listen(5)
+
import atexit
−
while True:
+
# create a default object, no changes to I2C address or frequency
−
     conn, addr = serv.accept()
+
mh = Raspi_MotorHAT(addr=0x6f)
−
     message_from_client = ''
+
# auto-disabling motors on shutdown
 +
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()
 +
 
 +
               
 +
#the main loop starts here
 +
if __name__ == '__main__':
 +
    #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:
 
     while True:
−
         data = conn.recv(4096)
+
         conn, addr = serv.accept()
−
        if not data: break
+
        message_from_client = ''
−
        message_from_client = data.decode()
+
        while (escape == True):
−
        print ("Tx:" + message_from_client)    
+
            data = conn.recv(4096)
−
        message = "RxAck:" + message_from_client
+
            if not data: break
−
        conn.send(message.encode())
+
            message_from_client = data.decode()
−
    conn.close()
+
            print ("Tx:" + message_from_client)
−
    if message_from_client == "Stop the Server" : break
+
            #receives a motor drive message
−
print ('Communication protocols termintated at Server by Client')
+
            if 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())
 +
               
 +
            else:
 +
                message = "RxAck:" + message_from_client
 +
                conn.send(message.encode())
 +
        conn.close()
 +
        if message_from_client[:3] == "SRS":
 +
            print ('Coms and Robot Control termintated at Server by Client')
 +
            break
 +
    #robot server system shutdown, terminate everything
 +
    turnOffMotors()
 +
else:
 +
    print ("Cannot be executed unless main")
 
</source>
 
</source>
Joelmartin
Bots, Bureaucrats, contributor, Interface administrators, lookupuser, Administrators
4,000

edits

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

Navigation menu