4,000
edits
Changes
Jump to navigation
Jump to search
mLine 334:
Line 334: − # -*- coding: utf-8 -*- − #author: joel.martin − #import the modules required here − import os − import socket − from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor − import time − import atexit − import MPU9250 − import sys − − #--------------------------------------------------------- − − # create a default object, no changes to I2C address or frequency − mh = Raspi_MotorHAT(addr=0x6f) − mpu9250 = MPU9250.MPU9250() − # 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: − conn, addr = serv.accept() − message_from_client = '' − while (escape == True): − #temp = mpu9250.readTemperature() − #print ("temp = ", temp) − data = conn.recv(4096) − if not data: break − message_from_client = data.decode() − print ("Tx:" + message_from_client) − #receives a motor drive message − if message_from_client[:3] == "ACC": − accel = mpu9250.readAccel() − #conn.send(message.encode()) − #print ("ax = ", (accel['x'])) − #print ("ay = ", (accel['y'])) − #print ("az = ", (accel['z'])) − 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() − #print ("gx = ", (gyro['x'])) − #print ("gy = ", (gyro['y'])) − #print ("gz = ", (gyro['z'])) − 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() − #print ("mx = ", (mag['x'])) − #print ("my = ", (mag['y'])) − #print ("mz = ", (mag['z'])) − 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()) − − 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")
→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">
</source>
</source>